# motions implements the motionInterface
from naoqi import ALProxy
import time
import math

motion = ALProxy('ALMotion', '127.0.0.1', 9559)
pose = ALProxy('ALRobotPose', '127.0.0.1', 9559)

def newHakje():
    normalPose()
    time.sleep(3)
    
    names = list()
    angles = list()
    times =  list()

    names.extend(['RElbowRoll', 'RElbowYaw', 'LElbowRoll', 'LElbowYaw'])
    angles.extend([[0], [0], [0], [0]])
    times.extend([[0.1], [0.1], [0.1], [0.1]])
    
    names.append('RShoulderPitch')
    angles.append( [ 1.2, 0.3 ])
    times.append([ 0.8, 2.0 ])

    names.append('RShoulderRoll')
    angles.append( [-0.3])
    times.append([0.1])
     
    names.append('LShoulderPitch')
    angles.append( [ 1.2  ])
    times.append([ 0.1 ])
    
    names.append('LShoulderRoll')
    angles.append( [0.3, 1])
    times.append([0.8, 2.0 ])
    
    names.append('LHipPitch',)
    angles.append( [-0.2, -0.4, -0.37, -0.2, -0.2,])
    times.append([  0.4,   0.8, 1.2 ,   1.6, 2.0, ])
    
    names.append('LKneePitch')
    angles.append( [0.4, 0.6, 0.54, 0.45, -0.1,])
    times.append([  0.4 ,0.8, 1.2, 1.6, 2.0,])
    
    names.append('LAnklePitch')
    angles.append( [-0.2, 0 , 0,   0,   0,]) 
    times.append([  0.4 ,0.8, 1.2, 1.6, 2.0,])

    names.append('LAnkleRoll')
    angles.append([ -0.2, -0.1, 0.2])
    times.append([ 0.8, 1.2, 2.0]) 
    
    names.append('RHipPitch')
    angles.append( [-0.2, 0.35, 0.26, 0.36])
    times.append( [  0.4,  0.8, 1.2, 1.6])

    names.append('RKneePitch')
    angles.append( [ 0.4, -0.25, 0.36, 0.])
    times.append( [  0.4,  0.8,  1.2,  1.6] )

    names.append( 'RAnklePitch')
    angles.append( [-0.2, -0.2, -0.44, -0.2])
    times.append([ 0.4,   0.8,   1.2,   1.6,])

    names.append('RAnkleRoll')
    angles.append( [-0.2, -0.1, 0.2])
    times.append([ 0.8, 1.2, 2.0]) 

    # prepare to kick
    motion.angleInterpolation(names, angles, times, True)
    time.sleep(3)
    normalPose()
    '''
    time.sleep(1)
    
    # check to see if nao has fallen
    if pose.getActualPoseAndTime()[0] == 'Back':
        kill()
        return 0
    elif pose.getActualPoseAndTime()[0] == 'Belly':
        kill()
        return 0
    
    # leg over ball, heel
    motion.angleInterpolation(['LHipYawPitch', 'RHipRoll', 'RHipPitch',  'RKneePitch', 'RAnklePitch','RShoulderPitch','LShoulderPitch'], \
                              [[angle, angle], [-0.5, 0],  [-1.2, -0.2], [0.2, 0.5],   [0.6, -0.4],   [1.5, 0.6],        [1.5, 0.6]  ], \
                              [[0.2, 0.8],     [0.3, 0.6], [0.7, 0.8],   [0.7, 0.9],   [0.7, 0.8],    [0.3, 0.8],     [0.3, 0.8]  ]    , True)
    motion.setAngles(['LShoulderRoll', 'RShoulderRoll', 'LShoulderPitch', 'RShoulderPitch'], [0.1, -0.1, 1.5, 1.5], 0.2)    
    motion.angleInterpolation(['LHipYawPitch', 'LHipRoll', 'LHipPitch', 'LKneePitch', 'LAnklePitch', 'LAnkleRoll', 'RHipRoll', 'RHipPitch', 'RKneePitch', 'RAnklePitch', 'RAnkleRoll'], \
                              [[0],         [0],        [-0.4],     [0.95],        [-0.55],       [0],          [0],        [-0.4],     [0.95],       [-0.55],       [0]], \
                              [[0.65],          [0.85],     [0.85],     [0.85],         [0.85],       [0.85],       [0.85],     [0.85],      [0.85],      [0.65],        [0.75]], True)
            
    motion.angleInterpolation('Body', angles, times, True)

    '''
# activate stiffness
def stiff():
    if motion.getStiffnesses('HeadPitch') != 0:
        motion.setStiffnesses(['RArm', 'LArm', 'LLeg', 'RLeg', 'Head'], [0.6, 0.6, 0.6, 0.6,0.6,0.6,0.6,0.6, 0.95,0.95,0.95,0.95,0.95,0.95, 0.95,0.95,0.95,0.95,0.95,0.95, 0.6,0.6])

# ##### Headmovements ######
# non blocking call, relative
def changeHead((yaw,pitch)):
    motion.changeAngles(['HeadYaw', 'HeadPitch'], [yaw, pitch], 0.3)

# non blocking call, absolute
def setHead((yaw, pitch)):
    motion.setAngles(['HeadYaw', 'HeadPitch'], [yaw, pitch], 0.5)

# blocking call (for scanCircle), absolute
def setHeadBlock((yaw, pitch, time)):
    motion.angleInterpolation(['HeadYaw', 'HeadPitch'], [[yaw, yaw, yaw], [pitch, pitch, pitch]], [[time - 0.05, time - 0.04, time], [time - 0.05, time-0.04, time]], True)

# ##### Walking ######
# setWalkTargetVelocity    
def SWTV((x,y,t,f)):
    motion.setWalkTargetVelocity(x,y,t,f)
    
# non-blocking walk call    
def postWalkTo((x, y, angle)):
    motion.post.walkTo(x, y, angle)    

# blocking walk call
def walkTo((x,y,angle)):
    motion.walkTo(x,y,angle)
            
# ##### 'advanced' motions #####    
# a normal pose from which walking is immediately possible    
def keepNormalPose():
    names = list()
    times = list()
    angles = list()
    
    if getKeepNormalPose():
        return True
    
    names.append('HeadPitch')
    times.append([1])
    angles.append([ [ -0.5 , [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])        
        
    names.append('HeadYaw')
    times.append([1])
    angles.append([ [ 0.0, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])
    
    names.append('LShoulderPitch')
    times.append([1])
    angles.append([ [ 1.2, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])

    names.append('LShoulderRoll')
    times.append([1])
    angles.append([ [ 0.15, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])

    names.append('LElbowYaw')
    times.append([1])
    angles.append([ [ 0, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])

    names.append('LElbowRoll')
    times.append([1])
    angles.append([ [ 0, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])

    names.append('RShoulderPitch')
    times.append([1])
    angles.append([ [ 1.2, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])

    names.append('RShoulderRoll')
    times.append([1])
    angles.append([ [ -0.15, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])

    names.append('RElbowYaw')
    times.append([1])
    angles.append([ [ 0, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])

    names.append('RElbowRoll')
    times.append([1])
    angles.append([ [ 0, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])

    names.append('LHipYawPitch')
    times.append([1])
    angles.append([ [ 0, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])

    names.append('LHipRoll')
    times.append([1])
    angles.append([ [ 0, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])

    names.append('LHipPitch')
    times.append([1])
    angles.append([ [ 0, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])

    names.append('LKneePitch')
    times.append([1])
    angles.append([ [ 0, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])

    names.append('LAnklePitch')
    times.append([1])
    angles.append([ [ 0, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])

    names.append('LAnkleRoll')
    times.append([1])
    angles.append([ [ 0.0, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])

    names.append('RHipRoll')
    times.append([1])
    angles.append([ [ 0.0, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])

    names.append('RHipPitch')
    times.append([1])
    angles.append([ [ 0, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])

    names.append('RKneePitch')
    times.append([1])
    angles.append([ [ 0, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])

    names.append('RAnklePitch')
    times.append([1])
    angles.append([ [ 0, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])

    names.append('RAnkleRoll')
    times.append([1])
    angles.append([ [ 0, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])

    motion.post.angleInterpolationBezier(names, times, angles)    
    
def normalPose():
    names = list()
    times = list()
    angles = list()
    
    if getNormalPose():
        return True
    
    names.append('HeadPitch')
    times.append([1])
    angles.append([ [ -0.5 , [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])        
        
    names.append('HeadYaw')
    times.append([1])
    angles.append([ [ 0.0, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])
    
    names.append('LShoulderPitch')
    times.append([1])
    angles.append([ [ 1.2, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])

    names.append('LShoulderRoll')
    times.append([1])
    angles.append([ [ 0.15, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])

    names.append('LElbowYaw')
    times.append([1])
    angles.append([ [ 0, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])

    names.append('LElbowRoll')
    times.append([1])
    angles.append([ [ 0, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])

    names.append('RShoulderPitch')
    times.append([1])
    angles.append([ [ 1.2, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])

    names.append('RShoulderRoll')
    times.append([1])
    angles.append([ [ -0.15, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])

    names.append('RElbowYaw')
    times.append([1])
    angles.append([ [ 0, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])

    names.append('RElbowRoll')
    times.append([1])
    angles.append([ [ 0, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])

    names.append('LHipYawPitch')
    times.append([1])
    angles.append([ [ 0, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])

    names.append('LHipRoll')
    times.append([1])
    angles.append([ [ 0, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])

    names.append('LHipPitch')
    times.append([1])
    angles.append([ [ -0.4, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])

    names.append('LKneePitch')
    times.append([1])
    angles.append([ [ 0.95, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])

    names.append('LAnklePitch')
    times.append([1])
    angles.append([ [ -0.55, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])

    names.append('LAnkleRoll')
    times.append([1])
    angles.append([ [ 0.0, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])

    names.append('RHipRoll')
    times.append([1])
    angles.append([ [ 0.0, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])

    names.append('RHipPitch')
    times.append([1])
    angles.append([ [ -0.4, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])

    names.append('RKneePitch')
    times.append([1])
    angles.append([ [ 0.95, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])

    names.append('RAnklePitch')
    times.append([1])
    angles.append([ [ -0.55, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])

    names.append('RAnkleRoll')
    times.append([1])
    angles.append([ [ 0.0, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])

    motion.post.angleInterpolationBezier(names, times, angles)    
    
def rKickAngled(angle):
    motion.setAngles(['LShoulderRoll', 'LShoulderPitch', 'RShoulderRoll', 'RShoulderPitch', 'RElbowRoll', 'RElbowYaw', 'LElbowRoll', 'LElbowYaw'], [0.5 + 0.2*angle, 0.8, -0.3, 0.8, 0, 0, 0, 0], 0.3)
    # Turn on left foot
    motion.setAngles(['LAnkleRoll', 'RAnkleRoll'], [0.185 + 0.025 * angle, 0.2], 0.2)    
    time.sleep(0.3)
    # Left leg stretched, right leg goes backwards
    motion.angleInterpolation(['LHipYawPitch', 'LHipRoll',  'LKneePitch',         'LAnklePitch',       'RHipRoll',          'LHipPitch',         'RHipPitch', 'RKneePitch',       'RAnklePitch'], \
                              [[0.65*angle],   [0.2*angle], [0.175 + 0.25*angle], [-0.125 -0.125*angle], [0.075 -0.295*angle], [-0.3 - 0.05*angle], [0.4],       [0.5 - 0.35*angle], [-0.6 + 0.5*angle]],\
                              [[0.8],          [0.9],       [0.6],                [0.6],               [1.1],               [0.6],               [1.1],       [1.2],              [1.2]], True)

    time.sleep(0.3)
    if pose.getActualPoseAndTime()[0] == 'Back':
        kill()
        return 0
    elif pose.getActualPoseAndTime()[0] == 'Belly':
        kill()
        return 0
    
    # kick
    motion.setAngles('RShoulderPitch', 2, 1)
    motion.setAngles(['LHipYawPitch', 'LHipRoll', 'LHipPitch', 'RHipPitch', 'RKneePitch', 'RAnklePitch'], [0.75*angle, 0.1, -0.1 -0.25*angle , -0.2 + -0.25 * angle, 0, 0.1 - 0.075*angle], 0.9)
    time.sleep(0.4 + 0.3 * angle)
    # return to start position
    motion.angleInterpolation(['LHipYawPitch', 'LHipRoll', 'LHipPitch', 'LKneePitch', 'LAnklePitch', 'LAnkleRoll', 'RHipRoll', 'RHipPitch', 'RKneePitch', 'RAnklePitch', 'RAnkleRoll'], [[0], [0],[-0.4], [0.95], [-0.55],[0.15], [0], [-0.4],  [0.95], [-0.55], [0.15]], [[0.65],[0.85],[0.85],[0.85],[0.85],[0.85],[0.85],[0.85],[0.85],[0.65],[0.75]], True)
    time.sleep(0.2)
    motion.setAngles(['LShoulderRoll', 'LShoulderPitch', 'RShoulderRoll', 'RShoulderPitch'], [0, 1.2, 0, 1.2], 0.4)
    motion.setAngles(['LAnkleRoll', 'RAnkleRoll'], [0, 0], 0.05)

def lKickAngled(angle):
    motion.setAngles(['RShoulderRoll', 'RShoulderPitch', 'LShoulderRoll', 'LShoulderPitch', 'RElbowRoll', 'RElbowYaw', 'LElbowRoll', 'LElbowYaw'], [-0.5 - 0.2*angle, 0.8, 0.3, 0.8, 0, 0, 0, 0], 0.3)
    # Turn on left foot
    motion.setAngles(['RAnkleRoll', 'LAnkleRoll'], [-0.185 - 0.025 * angle, -0.2], 0.2)    
    time.sleep(0.3)
    # Left leg stretched, right leg goes backwards
    motion.angleInterpolation(['LHipYawPitch', 'RHipRoll',  'RKneePitch',         'RAnklePitch',       'LHipRoll',           'RHipPitch',         'LHipPitch', 'LKneePitch',       'LAnklePitch'], \
                              [[0.65*angle],   [-0.2*angle], [0.175 + 0.25*angle], [-0.125 -0.125*angle], [-0.075 +0.295*angle], [-0.3 - 0.05*angle], [0.4],       [0.5 - 0.35*angle], [-0.6 + 0.5*angle]],\
                              [[0.8],          [0.9],       [0.6],                [0.6],                [1.1],                 [0.6],               [1.1],       [1.2],              [1.2]], True)

    time.sleep(0.3)
    
    if pose.getActualPoseAndTime()[0] == 'Back':
        kill()
        return 0
    elif pose.getActualPoseAndTime()[0] == 'Belly':
        kill()
        return 0
    # kick
    motion.setAngles('LShoulderPitch', 2, 1)
    motion.setAngles(['LHipYawPitch', 'RHipRoll', 'RHipPitch', 'LHipPitch', 'LKneePitch', 'LAnklePitch'], [0.75*angle, -0.1, -0.1 -0.25*angle , -0.2 + -0.25 * angle, 0, 0.1 - 0.075*angle], 0.9)
    time.sleep(0.4 + 0.3 * angle)
    # return to start position
    motion.angleInterpolation(['LHipYawPitch', 'RHipRoll', 'RHipPitch', 'RKneePitch', 'RAnklePitch', 'RAnkleRoll', 'LHipRoll', 'LHipPitch', 'LKneePitch', 'LAnklePitch', 'LAnkleRoll'], [[0], [0],[-0.4], [0.95], [-0.55],[-0.15], [0], [-0.4],  [0.95], [-0.55], [-0.15]], [[0.65],[0.85],[0.85],[0.85],[0.85],[0.85],[0.85],[0.85],[0.85],[0.65],[0.75]], True)
    time.sleep(0.2)
    motion.setAngles(['RShoulderRoll', 'RShoulderPitch', 'LShoulderRoll', 'LShoulderPitch'], [0, 1.2, 0, 1.2], 0.4)
    motion.setAngles(['RAnkleRoll', 'LAnkleRoll'], [0, 0], 0.05)

    # MAX ANGLE 0.3!!!!
def hakje(angle):
    # motion.setAngles(['RAnkleRoll', 'LAnkleRoll'], [-0.2, -0.2], 0.2)
    # time.sleep(0.2)
    # step
    motion.setStiffnesses('Body', 1)
    motion.setStiffnesses('Head', 0.5)
    
    names = list()
    angles = list()
    times =  list()

    names.extend(['RElbowRoll', 'RElbowYaw', 'LElbowRoll', 'LElbowYaw'])
    angles.extend([[0], [0], [0], [0]])
    times.extend([[0.1], [0.1], [0.1], [0.1]])
    
    names.append('RShoulderPitch')
    angles.append( [ 1.2,  0.8,   0.5, 0.8 ])
    times.append([ 0.1,    1.3,  2 , 3.5])

    names.append('RShoulderRoll')
    angles.append( [-0.2, -0.2])
    times.append([0.1, 1.1])
     
    names.append('LShoulderPitch')
    angles.append( [ 1.2,  0.8,   0.5, 0.8 ])
    times.append([ 0.1,    1.3,  2 , 3.5])
    
    names.append('LShoulderRoll')
    angles.append( [0.3, 0.4, 1])
    times.append([0.1, 0.8, 1.1])
    
    names.append('LHipPitch',)
    angles.append( [-0.5, -0.8,  -0.4, 0])
    times.append([0.4,   0.7,   1.1 ,  1.7])
    
    names.append('LKneePitch')
    angles.append( [0.95,  1.13,  0.5,   0 ])
    times.append([0.4,   0.7,   1.1,   1.4 ])
    
    names.append('LAnklePitch')
    angles.append( [-0.2,  -0.4,  -0.1,  -0.025,  0]) 
    times.append([0.4,   0.8,   1.2,   1.4,     2.5]) 

    names.append('LAnkleRoll')
    angles.append([-0.11,  0.13,   0.2, ])
    times.append([0.3,   0.95,     1.2, ]) 
    
    names.append('RHipPitch')
    angles.append([-0.4,   -0.22,   0.5,  0.5,  -1])
    times.append([0.4,    0.8,    1.2,  2,   3])

    names.append('RKneePitch')
    angles.append( [ 0.95,  0.95,   0.65,  2 ])
    times.append([ 0.4,   0.8,    1.2,   3])

    names.append( 'RAnklePitch')
    angles.append( [-0.5,  -0.67,  -1.1, -1 ])
    times.append([ 0.4,   0.8,    1.2, 3])

    names.append('RAnkleRoll')
    angles.append( [-0.11, -0.12,  0.2, ]) # 0.23,  0.225])
    times.append([ 0.3,   0.7,   1.1, ]) #  1.5,   2.3])    

    # prepare to kick
    motion.angleInterpolation(names, angles, times, True)

    time.sleep(1)
    
    # check to see if nao has fallen
    if pose.getActualPoseAndTime()[0] == 'Back':
        kill()
        return 0
    elif pose.getActualPoseAndTime()[0] == 'Belly':
        kill()
        return 0
    
    # leg over ball, heel
    motion.angleInterpolation(['LHipYawPitch', 'RHipRoll', 'RHipPitch',  'RKneePitch', 'RAnklePitch','RShoulderPitch','LShoulderPitch'], \
                              [[angle, angle], [-0.5, 0],  [-1.2, -0.2], [0.2, 0.5],   [0.6, -0.4],   [1.5, 0.6],        [1.5, 0.6]  ], \
                              [[0.2, 0.8],     [0.3, 0.6], [0.7, 0.8],   [0.7, 0.9],   [0.7, 0.8],    [0.3, 0.8],     [0.3, 0.8]  ]    , True)
    motion.setAngles(['LShoulderRoll', 'RShoulderRoll', 'LShoulderPitch', 'RShoulderPitch'], [0.1, -0.1, 1.5, 1.5], 0.2)    
    motion.angleInterpolation(['LHipYawPitch', 'LHipRoll', 'LHipPitch', 'LKneePitch', 'LAnklePitch', 'LAnkleRoll', 'RHipRoll', 'RHipPitch', 'RKneePitch', 'RAnklePitch', 'RAnkleRoll'], \
                              [[0],         [0],        [-0.4],     [0.95],        [-0.55],       [0],          [0],        [-0.4],     [0.95],       [-0.55],       [0]], \
                              [[0.65],          [0.85],     [0.85],     [0.85],         [0.85],       [0.85],       [0.85],     [0.85],      [0.85],      [0.65],        [0.75]], True)
    
    stiff()
    
def footLeft():
    names = list()
    times = list()
    angles = list()
    
    names.append('RShoulderPitch')
    times.append([0.75])
    angles.append([ [1.2, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])

    names.append('RShoulderRoll')
    times.append([0.75])
    angles.append([ [-0.43, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])

    names.append('RElbowYaw')
    times.append([0.75])
    angles.append([ [-0.61, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])

    names.append('RElbowRoll')
    times.append([0.75])
    angles.append([ [0.44, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])

    names.append('LHipYawPitch')
    times.append([0.6])
    angles.append([ [-0.98, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])

    names.append('RHipRoll')
    times.append([0.75])
    angles.append([ [0.1426, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])

    names.append('RHipPitch')
    times.append([0.75])
    angles.append([ [-0.2222, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])

    names.append('RKneePitch')
    times.append([0.75])
    angles.append([ [2.11, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])
    
    names.append('RAnklePitch')
    times.append([0.75])
    angles.append([ [-0.9, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])
    
    names.append('RAnkleRoll')
    times.append([0.75])
    angles.append([ [0.079, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])

    names.append('LHipRoll')
    times.append([0.75])
    angles.append([ [0.68, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])

    names.append('LHipPitch')
    times.append([0.75, 1.2])
    angles.append([ [-1.5, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]], [-1.1, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])

    names.append('LKneePitch')
    times.append([0.75, 1.2])
    angles.append([ [0.7, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]], [0, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])

    names.append('LAnklePitch')
    times.append([0.75, 1.2])
    angles.append([ [1, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]], [2, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])

    names.append('LAnkleRoll')
    times.append([0.75])
    angles.append([ [-0.05, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])    
    
    names.append('LShoulderPitch')
    times.append([0.4])
    angles.append([ [1.9, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])

    names.append('LShoulderRoll')
    times.append([0.4])
    angles.append([ [0.5, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])

    names.append('LElbowYaw')
    times.append([0.4])
    angles.append([ [-0.3, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])

    names.append('LElbowRoll')
    times.append([0.4])
    angles.append([ [-0.05, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])

    motion.angleInterpolationBezier(names, times, angles);
    
    # wait to stop the ball
    time.sleep(1)
    
    angles = list()
    times = list()
    
    '''
    converts an entire bodyAngleList from left- to right-oriented. 
    def function(input):
    allangle = list()
    for j in range(len(input)):
        oneangle = list()
        for i in range(len(input[0])):
            if i >= 2 and i <= 5:
                if i == 3 or i == 5:
                    oneangle.append(-input[j][i+16])
                else:
                    oneangle.append(input[j][i+16])
            elif i >= 6 and i <= 11:
                if i == 7 or i == 11:
                    oneangle.append(-input[j][i+6])
                else:
                    oneangle.append(input[j][i+6])
            elif i >= 12 and i <= 17:
                if i == 13 or i == 17:
                    oneangle.append(-input[j][i-6])
                else:
                    oneangle.append(input[j][i-6])
            elif i >= 18:
                if i == 19 or i == 21:
                    oneangle.append(-input[j][i-16])
                else:
                    oneangle.append(input[j][i-16])
            else:
                oneangle.append(input[j][i])
        allangle.append(oneangle)
    return allangle
    '''
    
    anglelist = [[0.056716039776802063, 0.31442803144454956, 1.9497560262680054, 0.4939899742603302, -0.30070596933364868, -0.09361596405506134, -0.95717406272888184, 0.68565607070922852, -1.3116120100021362, 0.69034194946289063, 0.93200582265853882, -0.001575961709022522, -0.95717406272888184, 0.14568804204463959, -0.24233004450798035, 2.112546443939209, -0.88209199905395508, 0.0076280385255813599, 1.3023240566253662, -0.33437004685401917, -0.61364197731018066, 0.42641004920005798],[0.05825003981590271, 0.3159620463848114, 1.9497560262680054, 0.50012600421905518, -0.29917198419570923, -0.090547963976860046, -0.96484404802322388, 0.68565607070922852, -1.5647220611572266, 1.2747960090637207, 0.6535259485244751, -4.1961669921875e-05, -0.96484404802322388, 0.14568804204463959, -0.21165004372596741, 2.112546443939209, -0.90816998481750488, 0.0076280385255813599, 1.3406740427017212, -0.33590406179428101, -0.61364197731018066, 0.42487603425979614], [0.056716039776802063, 0.31442803144454956, 1.9359500408172607, 0.50012600421905518, -0.30070596933364868, -0.085945963859558105, -0.97251403331756592, 0.68719005584716797, -1.5800620317459106, 1.4711480140686035, 0.45103797316551208, -0.18565596640110016, -0.97251403331756592, 0.14568804204463959, -0.20858204364776611, 2.112546443939209, -1.1229300498962402, 0.018366038799285889, 1.4465200901031494, -0.33437004685401917, -0.61364197731018066, 0.42487603425979614],[0.056716039776802063, 0.3159620463848114, 1.9359500408172607, 0.50012600421905518, -0.30070596933364868, -0.084411963820457458, -0.97251403331756592, 0.68719005584716797, -1.5800620317459106, 1.831637978553772, 0.1381019651889801, -0.19332596659660339, -0.97251403331756592, 0.14262004196643829, -0.20704804360866547, 2.112546443939209, -1.132133960723877, -0.016915962100028992, 1.4557241201400757, -0.33437004685401917, -0.61364197731018066, 0.42487603425979614], [0.056716039776802063, 0.3159620463848114, 1.9328820705413818, 0.50012600421905518, -0.30070596933364868, -0.084411963820457458, -0.98325204849243164, 0.68872404098510742, -1.5754599571228027, 2.112546443939209, -0.22392204403877258, -0.27616196870803833, -0.98325204849243164, 0.14415404200553894, -0.20858204364776611, 2.112546443939209, -1.1894419193267822, 0.0045600384473800659, 1.4833360910415649, -0.33437004685401917, -0.61057400703430176, 0.42487603425979614],[0.064386039972305298, 0.3159620463848114, 1.9313479661941528, 0.47711598873138428, -0.29917198419570923, -0.085945963859558105, -0.77616202831268311, 0.55066406726837158, -0.86521798372268677, 1.8837940692901611, -0.35431206226348877, -0.30837595462799072, -0.77616202831268311, -0.1381019651889801, -0.3451080322265625, 2.112546443939209, -1.1894419193267822, -0.10588795691728592, 1.6198620796203613, -0.33437004685401917, -0.61057400703430176, 0.42487603425979614],[0.065920040011405945, 0.31442803144454956, 1.9313479661941528, 0.47711598873138428, -0.29917198419570923, -0.085945963859558105, -0.74855005741119385, 0.41107004880905151, -0.53540796041488647, 2.0755441188812256, -0.88047409057617188, -0.22553996741771698, -0.74855005741119385, -0.16571396589279175, -0.37272006273269653, 2.112546443939209, -1.1894419193267822, -0.10435395687818527, 1.6290661096572876, -0.33437004685401917, -0.61057400703430176, 0.42487603425979614],[0.059784039855003357, 0.30982604622840881, 1.925212025642395, 0.4740479588508606, -0.29917198419570923, -0.085945963859558105, -0.75315207242965698, 0.40953606367111206, -0.55381596088409424, 2.1016221046447754, -0.92496007680892944, -0.16264596581459045, -0.75315207242965698, -0.17338396608829498, -0.38039004802703857, 2.112546443939209, -1.1894419193267822, -0.053731963038444519, 1.6428720951080322, -0.32363206148147583, -0.60903996229171753, 0.42180806398391724], [0.078192040324211121, 0.31289404630661011, 1.0615699291229248, 0.81612998247146606, -0.055265963077545166, -0.75783801078796387, -0.63656806945800781, 0.25153404474258423, -0.22247196733951569, 1.7825499773025513, -0.77002608776092529, -0.27769595384597778, -0.63656806945800781, -0.45410597324371338, -0.47396403551101685, 2.112546443939209, -1.1894419193267822, -0.050663962960243225, 1.688892126083374, -0.32670003175735474, -0.4418339729309082, 0.034906584769487381],[0.078192040324211121, 0.31289404630661011, 1.0631040334701538, 0.81612998247146606, -0.055265963077545166, -0.75937199592590332, -0.69639408588409424, 0.081260040402412415, -0.05833396315574646, 1.5233039855957031, -0.63350003957748413, -0.30990996956825256, -0.69639408588409424, -0.51853394508361816, -0.64423805475234985, 2.112546443939209, -1.1894419193267822, -0.10588795691728592, 1.6781541109085083, -0.32670003175735474, -0.44336795806884766, 0.034906584769487381],[0.081260040402412415, 0.31289404630661011, 1.0723079442977905, 0.80999398231506348, -0.053731963038444519, -0.76243996620178223, -0.60895603895187378, -0.14270396530628204, -0.1381019651889801, 1.7181220054626465, -1.101370096206665, -0.024585962295532227, -0.60895603895187378, -0.5952339768409729, -0.78996807336807251, 2.112546443939209, -1.1894419193267822, -0.099751964211463928, 1.6996300220489502, -0.5966840386390686, -0.44796997308731079, 0.034906584769487381], [0.081260040402412415, 0.31289404630661011, 1.0738420486450195, 0.80999398231506348, -0.053731963038444519, -0.76243996620178223, -0.31749606132507324, -0.37126997113227844, -0.50932997465133667, 1.8500460386276245, -1.1335840225219727, 0.12574604153633118, -0.31749606132507324, -0.45870798826217651, -0.81297802925109863, 2.112546443939209, -1.1894419193267822, -0.10435395687818527, 1.6996300220489502, -0.5966840386390686, -0.44796997308731079, 0.034906584769487381], [0.081260040402412415, 0.31289404630661011, 1.0738420486450195, 0.80999398231506348, -0.053731963038444519, -0.76243996620178223, -0.04291003942489624, -0.37126997113227844, -0.68573999404907227, 1.8193659782409668, -1.0768260955810547, 0.12267804145812988, -0.04291003942489624, -0.28536596894264221, -0.83752202987670898, 2.112546443939209, -1.1894419193267822, -0.092081964015960693, 1.6996300220489502, -0.5966840386390686, -0.44796997308731079, 0.034906584769487381]]
    for i in range(len(anglelist[0])):
                    # 1   2   3   4   5   6   7   8   9   10   11   12   13
        times.append([0.5, 1, 1.5, 2 ,2.5, 3, 3.5, 4, 4.3, 4.6, 4.9, 5.1,  5.5])
        
        oneangle = list()
        for j in range(len(anglelist)):
            oneangle.append(anglelist[j][i])
        angles.append(oneangle)
        
            
    motion.angleInterpolation('Body', angles, times, True)

    stance()

def setStiffnesses((name, arg)):
    motion.setStiffnesses(name, arg)

def footRight():
    names = list()
    times = list()
    angles = list()
    
    names.append('LShoulderPitch')
    times.append([0.75])
    angles.append([ [1.2, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])

    names.append('LShoulderRoll')
    times.append([0.75])
    angles.append([ [0.43, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])

    names.append('LElbowYaw')
    times.append([0.75])
    angles.append([ [-0.61, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])

    names.append('LElbowRoll')
    times.append([0.75])
    angles.append([ [-0.44, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])

    names.append('LHipYawPitch')
    times.append([0.6])
    angles.append([ [-0.98, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])

    names.append('LHipRoll')
    times.append([0.75])
    angles.append([ [-0.1426, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])

    names.append('LHipPitch')
    times.append([0.75])
    angles.append([ [-0.2222, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])

    names.append('LKneePitch')
    times.append([0.75])
    angles.append([ [2.11, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])
    
    names.append('LAnklePitch')
    times.append([0.75])
    angles.append([ [-0.9, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])
    
    names.append('LAnkleRoll')
    times.append([0.75])
    angles.append([ [-0.079, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])

    names.append('RHipRoll')
    times.append([0.75])
    angles.append([ [-0.68, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])

    names.append('RHipPitch')
    times.append([0.75, 1.2])
    angles.append([ [-1.5, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]], [-1.1, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])

    names.append('RKneePitch')
    times.append([0.75, 1.2])
    angles.append([ [0.7, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]], [0, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])

    names.append('RAnklePitch')
    times.append([0.75, 1.2])
    angles.append([ [1, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]], [2, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])

    names.append('RAnkleRoll')
    times.append([0.75])
    angles.append([ [0.05, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])    
    
    names.append('RShoulderPitch')
    times.append([0.4])
    angles.append([ [1.9, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])

    names.append('RShoulderRoll')
    times.append([0.4])
    angles.append([ [-0.5, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])

    names.append('RElbowYaw')
    times.append([0.4])
    angles.append([ [-0.3, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])

    names.append('RElbowRoll')
    times.append([0.4])
    angles.append([ [-0.05, [ 3, -1.00000, 0.00000], [ 3, 0.00000, 0.00000]]])

    motion.angleInterpolationBezier(names, times, angles);
    
    # wait to stop the ball
    time.sleep(1)
    
    angles = list()
    times = list()
    
    anglelist = [[0.056716039776802063, 0.31442803144454956, 1.3023240566253662, 0.33437004685401917, -0.61364197731018066, -0.42641004920005798, -0.95717406272888184, -0.14568804204463959, -0.24233004450798035, 2.112546443939209, -0.88209199905395508, -0.0076280385255813599, -0.95717406272888184, -0.68565607070922852, -1.3116120100021362, 0.69034194946289062, 0.93200582265853882, 0.001575961709022522, 1.9497560262680054, -0.4939899742603302, -0.30070596933364868, 0.09361596405506134],
                [0.05825003981590271, 0.3159620463848114, 1.3406740427017212, 0.33590406179428101, -0.61364197731018066, -0.42487603425979614, -0.96484404802322388, -0.14568804204463959, -0.21165004372596741, 2.112546443939209, -0.90816998481750488, -0.0076280385255813599, -0.96484404802322388, -0.68565607070922852, -1.5647220611572266, 1.2747960090637207, 0.6535259485244751, 4.1961669921875e-05, 1.9497560262680054, -0.50012600421905518, -0.29917198419570923, 0.090547963976860046],
                [0.056716039776802063, 0.31442803144454956, 1.4465200901031494, 0.33437004685401917, -0.61364197731018066, -0.42487603425979614, -0.97251403331756592, -0.14568804204463959, -0.20858204364776611, 2.112546443939209, -1.1229300498962402, -0.018366038799285889, -0.97251403331756592, -0.68719005584716797, -1.5800620317459106, 1.4711480140686035, 0.45103797316551208, 0.18565596640110016, 1.9359500408172607, -0.50012600421905518, -0.30070596933364868, 0.085945963859558105],
                [0.056716039776802063, 0.3159620463848114, 1.4557241201400757, 0.33437004685401917, -0.61364197731018066, -0.42487603425979614, -0.97251403331756592, -0.14262004196643829, -0.20704804360866547, 2.112546443939209, -1.132133960723877, 0.016915962100028992, -0.97251403331756592, -0.68719005584716797, -1.5800620317459106, 1.831637978553772, 0.1381019651889801, 0.19332596659660339, 1.9359500408172607, -0.50012600421905518, -0.30070596933364868, 0.084411963820457458],
                [0.056716039776802063, 0.3159620463848114, 1.4833360910415649, 0.33437004685401917, -0.61057400703430176, -0.42487603425979614, -0.98325204849243164, -0.14415404200553894, -0.20858204364776611, 2.112546443939209, -1.1894419193267822, -0.0045600384473800659, -0.98325204849243164, -0.68872404098510742, -1.5754599571228027, 2.112546443939209, -0.22392204403877258, 0.27616196870803833, 1.9328820705413818, -0.50012600421905518, -0.30070596933364868, 0.084411963820457458],
                [0.064386039972305298, 0.3159620463848114, 1.6198620796203613, 0.33437004685401917, -0.61057400703430176, -0.42487603425979614, -0.77616202831268311, 0.1381019651889801, -0.3451080322265625, 2.112546443939209, -1.1894419193267822, 0.10588795691728592, -0.77616202831268311, -0.55066406726837158, -0.86521798372268677, 1.8837940692901611, -0.35431206226348877, 0.30837595462799072, 1.9313479661941528, -0.47711598873138428, -0.29917198419570923, 0.085945963859558105],
                [0.065920040011405945, 0.31442803144454956, 1.6290661096572876, 0.33437004685401917, -0.61057400703430176, -0.42487603425979614, -0.74855005741119385, 0.16571396589279175, -0.37272006273269653, 2.112546443939209, -1.1894419193267822, 0.10435395687818527, -0.74855005741119385, -0.41107004880905151, -0.53540796041488647, 2.0755441188812256, -0.88047409057617188, 0.22553996741771698, 1.9313479661941528, -0.47711598873138428, -0.29917198419570923, 0.085945963859558105],
                [0.059784039855003357, 0.30982604622840881, 1.6428720951080322, 0.32363206148147583, -0.60903996229171753, -0.42180806398391724, -0.75315207242965698, 0.17338396608829498, -0.38039004802703857, 2.112546443939209, -1.1894419193267822, 0.053731963038444519, -0.75315207242965698, -0.40953606367111206, -0.55381596088409424, 2.1016221046447754, -0.92496007680892944, 0.16264596581459045, 1.925212025642395, -0.4740479588508606, -0.29917198419570923, 0.085945963859558105],
                [0.078192040324211121, 0.31289404630661011, 1.688892126083374, 0.32670003175735474, -0.4418339729309082, -0.034906584769487381, -0.63656806945800781, 0.45410597324371338, -0.47396403551101685, 2.112546443939209, -1.1894419193267822, 0.050663962960243225, -0.63656806945800781, -0.25153404474258423, -0.22247196733951569, 1.7825499773025513, -0.77002608776092529, 0.27769595384597778, 1.0615699291229248, -0.81612998247146606, -0.055265963077545166, 0.75783801078796387],
                [0.078192040324211121, 0.31289404630661011, 1.6781541109085083, 0.32670003175735474, -0.44336795806884766, -0.034906584769487381, -0.69639408588409424, 0.51853394508361816, -0.64423805475234985, 2.112546443939209, -1.1894419193267822, 0.10588795691728592, -0.69639408588409424, -0.081260040402412415, -0.05833396315574646, 1.5233039855957031, -0.63350003957748413, 0.30990996956825256, 1.0631040334701538, -0.81612998247146606, -0.055265963077545166, 0.75937199592590332],
                [0.081260040402412415, 0.31289404630661011, 1.6996300220489502, 0.5966840386390686, -0.44796997308731079, -0.034906584769487381, -0.60895603895187378, 0.5952339768409729, -0.78996807336807251, 2.112546443939209, -1.1894419193267822, 0.099751964211463928, -0.60895603895187378, 0.14270396530628204, -0.1381019651889801, 1.7181220054626465, -1.101370096206665, 0.024585962295532227, 1.0723079442977905, -0.80999398231506348, -0.053731963038444519, 0.76243996620178223],
                [0.081260040402412415, 0.31289404630661011, 1.6996300220489502, 0.5966840386390686, -0.44796997308731079, -0.034906584769487381, -0.31749606132507324, 0.45870798826217651, -0.81297802925109863, 2.112546443939209, -1.1894419193267822, 0.10435395687818527, -0.31749606132507324, 0.37126997113227844, -0.50932997465133667, 1.8500460386276245, -1.1335840225219727, -0.12574604153633118, 1.0738420486450195, -0.80999398231506348, -0.053731963038444519, 0.76243996620178223],
                [0.081260040402412415, 0.31289404630661011, 1.6996300220489502, 0.5966840386390686, -0.44796997308731079, -0.034906584769487381, -0.04291003942489624, 0.28536596894264221, -0.83752202987670898, 2.112546443939209, -1.1894419193267822, 0.092081964015960693, -0.04291003942489624, 0.37126997113227844, -0.68573999404907227, 1.8193659782409668, -1.0768260955810547, -0.12267804145812988, 1.0738420486450195, -0.80999398231506348, -0.053731963038444519, 0.76243996620178223]]        
                
    for i in range(len(anglelist[0])):
                    # 1   2   3   4   5   6   7   8   9   10   11   12   13
        times.append([0.5, 1, 1.5, 2 ,2.5, 3, 3.5, 4, 4.3, 4.6, 4.9, 5.1,  5.5])
        
        oneangle = list()
        for j in range(len(anglelist)):
            oneangle.append(anglelist[j][i])
        angles.append(oneangle)
        
            
    motion.angleInterpolation('Body', angles, times, True)

    stance()
    
def diveLeft():
    names = list()
    angles = list()
    times = list()
    
    names.extend(['LHipRoll', 'LHipPitch', 'LKneePitch', 'LAnklePitch', 'LAnkleRoll'])
    angles.extend([[0.438], [-0.8], [2.11], [-1.186], [-0.09]])
    times.extend([[0.5], [0.5], [0.5], [0.5], [0.5]])
    
    names.append('LHipYawPitch')
    angles.append([0.1])
    times.append([0.5])
    
    names.extend(['RHipRoll', 'RHipPitch', 'RKneePitch', 'RAnklePitch', 'RAnkleRoll'])
    angles.extend([[0], [-0.3], [0.6871], [-0.3099], [0.54788]])
    times.extend([[0.5], [0.5], [0.5], [0.5], [0.5]])

    names.extend(['LShoulderPitch', 'LShoulderRoll', 'LElbowYaw', 'LElbowRoll'])    
    angles.extend([[-1.1], [0.5], [0.1883], [-0.5016]])
    times.extend([[0.2], [0.2], [0.2], [0.2]])

    names.extend(['RShoulderPitch','RShoulderRoll', 'RElbowYaw', 'RElbowRoll'])
    angles.extend([[1.2, 1.5], [-0.11, 0.3403], [-0.1, 0.1226], [0.25, 0.903]])
    times.extend([[0.2,0.5], [0.2,0.5], [0.2,0.5], [0.2, 0.5]])
    
    motion.angleInterpolation(names, angles, times, True)
    
    motion.setStiffnesses('Body', 0)
    time.sleep(2)
    
    motion.setStiffnesses('RArm', 0.8)
    motion.setStiffnesses('LLeg', 0.6)
    motion.setStiffnesses('RLeg', 0.6)
    
    # move arm around body to shift balance
    motion.angleInterpolation(['RShoulderPitch','RShoulderRoll', 'RElbowYaw', 'RElbowRoll'], [[1.372, 2.085], [-1.2916, -0.105], [-0.154, -0.130], [1.1888, 0.380]], [[0.4, 0.8], [0.4, 0.8], [0.4, 0.8], [0.4, 0.8]], True)
    motion.setAngles('RLeg', [-0.5,0,0,0.6,0.2,-0.2], 0.2)    
    motion.setAngles('LLeg', [-0.5,0,0,0.6,0.2, 0.2], 0.2)
    motion.setStiffnesses('RArm', 0)
    while pose.getActualPoseAndTime()[0] != 'Back' and pose.getActualPoseAndTime()[0] != 'Belly':
        pass
    motion.setStiffnesses('Body',0.8)
    if pose.getActualPoseAndTime()[0] == 'Back':
        backToStand()
        motion.walkTo(0.1, 0.05, 1.5)

    elif pose.getActualPoseAndTime()[0] == 'Belly':
        bellyToStand()
        motion.walkTo(-0.1, -0.05, -1.5)
    
def diveRight():
    names = list()
    angles = list()
    times = list()
    
    names.extend(['RHipRoll', 'RHipPitch', 'RKneePitch', 'RAnklePitch', 'RAnkleRoll'])
    angles.extend([[-0.438], [-0.8], [2.11], [-1.186], [0.09]])
    times.extend([[0.5], [0.5], [0.5], [0.5], [0.5]])
    
    names.append('LHipYawPitch')
    angles.append([0.1])
    times.append([0.5])
    
    names.extend(['LHipRoll', 'LHipPitch', 'LKneePitch', 'LAnklePitch', 'LAnkleRoll'])
    angles.extend([[0], [-0.3], [0.6871], [-0.3099], [-0.54788]])
    times.extend([[0.5], [0.5], [0.5], [0.5], [0.5]])
    
    names.extend(['RShoulderPitch','RShoulderRoll', 'RElbowYaw', 'RElbowRoll'])
    angles.extend([[-1.1], [-0.5], [0.1883], [0.5016]])
    times.extend([[0.2], [0.2], [0.2], [0.2]])
    
    names.extend(['LShoulderPitch', 'LShoulderRoll', 'LElbowYaw', 'LElbowRoll'])
    angles.extend([[1.2, 1.5], [0.11, -0.3403], [-0.1, 0.1226], [-0.25, -0.903]])
    times.extend([[0.2,0.5], [0.2,0.5], [0.2,0.5], [0.2, 0.5]])
    
    motion.angleInterpolation(names, angles, times, True)
    motion.setStiffnesses('Body', 0)
    time.sleep(2)
    motion.setStiffnesses('LArm', 0.8)
    motion.setStiffnesses('LLeg', 0.6)
    motion.setStiffnesses('RLeg', 0.6)
    motion.angleInterpolation(['LShoulderPitch','LShoulderRoll', 'LElbowYaw', 'LElbowRoll'], [[1.372, 2.085], [1.2916, 0.105], [-0.154, -0.130], [-1.1888, -0.380]], [[0.4, 0.8], [0.4, 0.8], [0.4, 0.8], [0.4, 0.8]], True)
    motion.setAngles('LLeg', [-0.5,0,0,0.6,0.2, 0.2], 0.2)    
    motion.setAngles('RLeg', [-0.5,0,0,0.6,0.2,-0.2], 0.2)
    motion.setStiffnesses('RArm', 0)
    while pose.getActualPoseAndTime()[0] != 'Back' and pose.getActualPoseAndTime()[0] != 'Belly':
        pass    
    motion.setStiffnesses('Body',0.8)
    if pose.getActualPoseAndTime()[0] == 'Back':
        backToStand()
        motion.walkTo(0.1, -0.05, -1.5)
    elif pose.getActualPoseAndTime()[0] == 'Belly':
        bellyToStand()
        motion.walkTo(-0.1, 0.05, 1.5)
    
# Stand up from the back
def backToStand():
    names = list()
    times = list()
    angles = list()

    names.append('HeadYaw')
    times.append([0.5, 1.0, 1.4, 1.8, 2.4, 2.8, 3.3, 3.8, 4.5, 5.2, 5.9, 7.0])
    angles.append([ [ -0.02459, [ 3, -0.33333, 0.00000], [ 3, 0.33333, 0.00000]], [ -0.00000, [ 3, -0.33333, -0.01222], [ 3, 0.26667, 0.00977]], [ 0.04138, [ 3, -0.26667, 0.00000], [ 3, 0.26667, 0.00000]], [ 0.00000, [ 3, -0.26667, 0.00000], [ 3, 0.23333, -0.00000]], [ 0.00000, [ 3, -0.23333, 0.00000], [ 3, 0.16667, -0.00000]], [ 0.00000, [ 3, -0.16667, 0.00000], [ 3, 0.26667, -0.00000]], [ -0.51393, [ 3, -0.26667, 0.00000], [ 3, 0.30000, 0.00000]], [ -0.30224, [ 3, -0.30000, -0.12958], [ 3, 0.30000, 0.12958]], [ 0.26354, [ 3, -0.30000, 0.00000], [ 3, 0.20000, 0.00000]], [ 0.12043, [ 3, -0.20000, 0.04549], [ 3, 0.20000, -0.04549]], [ -0.00940, [ 3, -0.20000, 0.00000], [ 3, 0.63333, 0.00000]], [ 0.0, [ 3, -0.63333, 0.00000], [ 3, 0.00000, 0.00000]]])

    names.append('HeadPitch')
    times.append([0.5, 1.0, 1.4, 1.8, 2.4, 2.8, 3.3, 3.8, 4.5, 5.2, 5.9, 7.0])
    angles.append([ [ 0.06285, [ 3, -0.33333, 0.00000], [ 3, 0.33333, 0.00000]], [ 0.00000, [ 3, -0.33333, 0.00000], [ 3, 0.26667, 0.00000]], [ 0.51490, [ 3, -0.26667, 0.00000], [ 3, 0.26667, 0.00000]], [ -0.67200, [ 3, -0.26667, 0.00000], [ 3, 0.23333, 0.00000]], [ 0.00000, [ 3, -0.23333, -0.19854], [ 3, 0.16667, 0.14181]], [ 0.34907, [ 3, -0.16667, 0.00000], [ 3, 0.26667, 0.00000]], [ 0.17177, [ 3, -0.26667, 0.00000], [ 3, 0.30000, 0.00000]], [ 0.34511, [ 3, -0.30000, 0.00000], [ 3, 0.30000, 0.00000]], [ 0.01047, [ 3, -0.30000, 0.00000], [ 3, 0.20000, 0.00000]], [ 0.02967, [ 3, -0.20000, -0.01920], [ 3, 0.20000, 0.01920]], [ 0.37886, [ 3, -0.20000, 0.00000], [ 3, 0.63333, 0.00000]], [ -0.5, [ 3, -0.63333, 0.00000], [ 3, 0.00000, 0.00000]]])

    names.append('LShoulderPitch')
    times.append([0.5, 1.0, 1.4, 1.8, 2.4, 2.8, 3.3, 3.8, 4.5, 5.2, 5.9, 7.0])
    angles.append([ [ 2.08560, [ 3, -0.33333, 0.00000], [ 3, 0.33333, 0.00000]], [ 2.09440, [ 3, -0.33333, 0.00000], [ 3, 0.26667, 0.00000]], [ 2.03865, [ 3, -0.26667, 0.00000], [ 3, 0.26667, 0.00000]], [ 2.08560, [ 3, -0.26667, 0.00000], [ 3, 0.23333, 0.00000]], [ 2.08560, [ 3, -0.23333, 0.00000], [ 3, 0.16667, 0.00000]], [ 2.08560, [ 3, -0.16667, 0.00000], [ 3, 0.26667, 0.00000]], [ 2.03251, [ 3, -0.26667, 0.05309], [ 3, 0.30000, -0.05973]], [ 1.28698, [ 3, -0.30000, 0.17922], [ 3, 0.30000, -0.17922]], [ 0.95717, [ 3, -0.30000, 0.11076], [ 3, 0.20000, -0.07384]], [ 0.73321, [ 3, -0.20000, 0.00000], [ 3, 0.20000, 0.00000]], [ 0.73321, [ 3, -0.20000, 0.00000], [ 3, 0.63333, 0.00000]], [ 1.59225, [ 3, -0.63333, 0.00000], [ 3, 0.00000, 0.00000]]])

    names.append('LShoulderRoll')
    times.append([0.5, 1.0, 1.4, 1.8, 2.4, 2.8, 3.3, 3.8, 4.5, 5.2, 5.9, 7.0])
    angles.append([ [ 0.98018, [ 3, -0.33333, 0.00000], [ 3, 0.33333, 0.00000]], [ 0.36652, [ 3, -0.33333, 0.02182], [ 3, 0.26667, -0.01745]], [ 0.34907, [ 3, -0.26667, 0.00000], [ 3, 0.26667, 0.00000]], [ 0.34907, [ 3, -0.26667, 0.00000], [ 3, 0.23333, 0.00000]], [ 0.36652, [ 3, -0.23333, 0.00000], [ 3, 0.16667, 0.00000]], [ 0.00870, [ 3, -0.16667, 0.00000], [ 3, 0.26667, 0.00000]], [ 0.29909, [ 3, -0.26667, 0.00000], [ 3, 0.30000, 0.00000]], [ 0.07052, [ 3, -0.30000, 0.00000], [ 3, 0.30000, 0.00000]], [ 0.26687, [ 3, -0.30000, -0.08560], [ 3, 0.20000, 0.05706]], [ 0.49851, [ 3, -0.20000, 0.00000], [ 3, 0.20000, 0.00000]], [ 0.49851, [ 3, -0.20000, 0.00000], [ 3, 0.63333, 0.00000]], [ 0.21932, [ 3, -0.63333, 0.00000], [ 3, 0.00000, 0.00000]]])
    
    names.append('LElbowYaw')
    times.append([0.5, 1.0, 1.4, 1.8, 2.4, 2.8, 3.3, 3.8, 4.5, 5.2, 5.9, 7.0])
    angles.append([ [ -0.76091, [ 3, -0.33333, 0.00000], [ 3, 0.33333, 0.00000]], [ 0.15708, [ 3, -0.33333, 0.00000], [ 3, 0.26667, 0.00000]], [ 0.12268, [ 3, -0.26667, 0.01164], [ 3, 0.26667, -0.01164]], [ 0.08727, [ 3, -0.26667, 0.00000], [ 3, 0.23333, 0.00000]], [ 0.08727, [ 3, -0.23333, 0.00000], [ 3, 0.16667, 0.00000]], [ 0.08727, [ 3, -0.16667, 0.00000], [ 3, 0.26667, 0.00000]], [ -1.96049, [ 3, -0.26667, 0.00000], [ 3, 0.30000, 0.00000]], [ -0.85448, [ 3, -0.30000, -0.28507], [ 3, 0.30000, 0.28507]], [ -0.25008, [ 3, -0.30000, -0.18751], [ 3, 0.20000, 0.12501]], [ 0.08308, [ 3, -0.20000, 0.00000], [ 3, 0.20000, 0.00000]], [ 0.08308, [ 3, -0.20000, 0.00000], [ 3, 0.63333, 0.00000]], [0, [ 3, -0.63333, 0.00000], [ 3, 0.00000, 0.00000]]])

    names.append('LElbowRoll')
    times.append([0.5, 0.6, 0.8, 1.2, 2.4, 2.8, 3.3, 3.8, 4.5, 5.2, 5.9, 7.0])
    angles.append([ [ -0.62430, [ 3, -0.33333, 0.00000], [ 3, 0.33333, 0.00000]], [ 0.00000, [ 3, -0.33333, 0.00000], [ 3, 0.26667, 0.00000]], [ -1.56210, [ 3, -0.26667, 0.00000], [ 3, 0.26667, 0.00000]], [ -1.56210, [ 3, -0.26667, 0.00000], [ 3, 0.23333, 0.00000]], [ -0.69813, [ 3, -0.23333, -0.30205], [ 3, 0.16667, 0.21575]], [ -0.00870, [ 3, -0.16667, 0.00000], [ 3, 0.13333, 0.00000]], [ -1.04615, [ 3, -0.13333, 0.00000], [ 3, 0.30000, 0.00000]], [ -0.87280, [ 3, -0.30000, -0.03605], [ 3, 0.30000, 0.03605]], [ -0.82985, [ 3, -0.30000, -0.01349], [ 3, 0.20000, 0.00899]], [ -0.80535, [ 3, -0.20000, 0.00000], [ 3, 0.20000, 0.00000]], [ -0.80535, [ 3, -0.20000, 0.00000], [ 3, 0.63333, 0.00000]], [ 0, [ 3, -0.63333, 0.00000], [ 3, 0.00000, 0.00000]]])

    names.append('RShoulderPitch')
    times.append([0.5, 1.0, 1.4, 1.8, 2.4, 2.8, 3.3, 3.8, 4.5, 5.2, 5.9, 7.0])
    angles.append([ [ 2.08560, [ 3, -0.33333, 0.00000], [ 3, 0.33333, 0.00000]], [ 2.09440, [ 3, -0.33333, 0.00000], [ 3, 0.26667, 0.00000]], [ 1.95283, [ 3, -0.26667, 0.00000], [ 3, 0.26667, 0.00000]], [ 2.08560, [ 3, -0.26667, 0.00000], [ 3, 0.23333, 0.00000]], [ 2.08560, [ 3, -0.23333, 0.00000], [ 3, 0.16667, 0.00000]], [ 2.08560, [ 3, -0.16667, 0.00000], [ 3, 0.26667, 0.00000]], [ 2.07708, [ 3, -0.26667, 0.00759], [ 3, 0.30000, -0.00854]], [ 2.03720, [ 3, -0.30000, 0.03988], [ 3, 0.30000, -0.03988]], [ 1.44047, [ 3, -0.30000, 0.22918], [ 3, 0.20000, -0.15279]], [ 0.89131, [ 3, -0.20000, 0.00000], [ 3, 0.20000, 0.00000]], [ 0.89131, [ 3, -0.20000, 0.00000], [ 3, 0.63333, 0.00000]], [ 1.46961, [ 3, -0.63333, 0.00000], [ 3, 0.00000, 0.00000]]])
    
    names.append('RShoulderRoll')
    times.append([0.5, 1.0, 1.4, 1.8, 2.4, 2.8, 3.3, 3.8, 4.5, 5.2, 5.9, 7.0])
    angles.append([ [ -1.07384, [ 3, -0.33333, 0.00000], [ 3, 0.33333, 0.00000]], [ -0.36652, [ 3, -0.33333, -0.02182], [ 3, 0.26667, 0.01745]], [ -0.34907, [ 3, -0.26667, 0.00000], [ 3, 0.26667, 0.00000]], [ -0.34907, [ 3, -0.26667, 0.00000], [ 3, 0.23333, 0.00000]], [ -0.36652, [ 3, -0.23333, 0.00000], [ 3, 0.16667, 0.00000]], [ -0.00870, [ 3, -0.16667, 0.00000], [ 3, 0.26667, 0.00000]], [ -0.45717, [ 3, -0.26667, 0.08719], [ 3, 0.30000, -0.09809]], [ -0.56455, [ 3, -0.30000, 0.04142], [ 3, 0.30000, -0.04142]], [ -0.70568, [ 3, -0.30000, 0.06162], [ 3, 0.20000, -0.04108]], [ -0.87266, [ 3, -0.20000, 0.00000], [ 3, 0.20000, 0.00000]], [ -0.68068, [ 3, -0.20000, -0.05778], [ 3, 0.63333, 0.18298]], [ -0.15037, [ 3, -0.63333, 0.00000], [ 3, 0.00000, 0.00000]]])

    names.append('RElbowYaw')
    times.append([0.5, 1.0, 1.4, 1.8, 2.4, 2.8, 3.3, 3.8, 4.5, 5.2, 5.9, 7.0])
    angles.append([ [ 0.76542, [ 3, -0.33333, 0.00000], [ 3, 0.33333, 0.00000]], [ -0.15708, [ 3, -0.33333, 0.12584], [ 3, 0.26667, -0.10067]], [ -0.25775, [ 3, -0.26667, 0.00000], [ 3, 0.26667, 0.00000]], [ -0.08727, [ 3, -0.26667, 0.00000], [ 3, 0.23333, 0.00000]], [ -0.08727, [ 3, -0.23333, 0.00000], [ 3, 0.16667, 0.00000]], [ -0.08727, [ 3, -0.16667, 0.00000], [ 3, 0.26667, 0.00000]], [ -0.08134, [ 3, -0.26667, -0.00285], [ 3, 0.30000, 0.00321]], [ -0.06907, [ 3, -0.30000, -0.00920], [ 3, 0.30000, 0.00920]], [ -0.02612, [ 3, -0.30000, -0.01394], [ 3, 0.20000, 0.00929]], [ 0.00062, [ 3, -0.20000, 0.00000], [ 3, 0.20000, 0.00000]], [ 0.00062, [ 3, -0.20000, 0.00000], [ 3, 0.63333, 0.00000]], [ 0, [ 3, -0.63333, 0.00000], [ 3, 0.00000, 0.00000]]])

    names.append('RElbowRoll')
    times.append([0.5, 0.6, 0.8, 1.2, 2.4, 2.8, 3.3, 3.8, 4.5, 5.2, 5.9, 7.0])
    angles.append([ [ 0.71028, [ 3, -0.33333, 0.00000], [ 3, 0.33333, 0.00000]], [ 0.00000, [ 3, -0.33333, 0.00000], [ 3, 0.26667, 0.00000]], [ 1.56210, [ 3, -0.26667, 0.00000], [ 3, 0.26667, 0.00000]], [ 1.56210, [ 3, -0.26667, 0.00000], [ 3, 0.23333, 0.00000]], [ 0.69813, [ 3, -0.23333, 0.30205], [ 3, 0.16667, -0.21575]], [ 0.00870, [ 3, -0.16667, 0.00000], [ 3, 0.26667, 0.00000]], [ 0.09668, [ 3, -0.26667, 0.00000], [ 3, 0.30000, 0.00000]], [ 0.02152, [ 3, -0.30000, 0.00000], [ 3, 0.30000, 0.00000]], [ 0.19026, [ 3, -0.30000, -0.08645], [ 3, 0.20000, 0.05764]], [ 0.45379, [ 3, -0.20000, -0.06162], [ 3, 0.20000, 0.06162]], [ 0.55995, [ 3, -0.20000, 0.00000], [ 3, 0.63333, 0.00000]], [ 0, [ 3, -0.63333, 0.00000], [ 3, 0.00000, 0.00000]]])

    names.append('LHipYawPitch')
    times.append([0.5, 1.0, 1.4, 1.8, 2.4, 2.8, 3.3, 3.8, 4.5, 5.2, 5.9, 7.0])
    angles.append([ [ -0.47090, [ 3, -0.33333, 0.00000], [ 3, 0.33333, 0.00000]], [ -0.25090, [ 3, -0.33333, 0.00000], [ 3, 0.33333, 0.00000]], [ 0.00000, [ 3, -0.33333, -0.00964], [ 3, 0.26667, 0.00771]], [ 0.00771, [ 3, -0.26667, 0.00000], [ 3, 0.26667, 0.00000]], [ -0.00000, [ 3, -0.26667, 0.00771], [ 3, 0.23333, -0.00675]], [ -0.65498, [ 3, -0.23333, 0.00000], [ 3, 0.43333, 0.00000]], [ -0.49851, [ 3, -0.43333, 0.00000], [ 3, 0.30000, 0.00000]], [ -0.85900, [ 3, -0.30000, 0.00000], [ 3, 0.30000, 0.00000]], [ -0.69639, [ 3, -0.30000, -0.09135], [ 3, 0.20000, 0.06090]], [ -0.40225, [ 3, -0.20000, 0.00000], [ 3, 0.20000, 0.00000]], [ -0.40225, [ 3, -0.20000, 0.00000], [ 3, 0.63333, 0.00000]], [ 0, [ 3, -0.63333, 0.00000], [ 3, 0.00000, 0.00000]]])

    names.append('LHipRoll')
    times.append([0.5, 1.0, 1.4, 1.8, 2.4, 2.8, 3.3, 3.8, 4.5, 5.2, 5.9, 7.0])
    angles.append([ [ 0.04453, [ 3, -0.33333, 0.00000], [ 3, 0.33333, 0.00000]], [ 0.00000, [ 3, -0.33333, 0.00000], [ 3, 0.26667, 0.00000]], [ 0.05833, [ 3, -0.26667, 0.00000], [ 3, 0.26667, 0.00000]], [ 0.00000, [ 3, -0.26667, 0.02189], [ 3, 0.23333, -0.01915]], [ -0.06478, [ 3, -0.23333, 0.00000], [ 3, 0.16667, 0.00000]], [ 0.54105, [ 3, -0.16667, 0.00000], [ 3, 0.26667, 0.00000]], [ 0.15498, [ 3, -0.26667, 0.11230], [ 3, 0.30000, -0.12633]], [ -0.17483, [ 3, -0.30000, 0.00000], [ 3, 0.30000, 0.00000]], [ 0.00925, [ 3, -0.30000, -0.07336], [ 3, 0.20000, 0.04891]], [ 0.19199, [ 3, -0.20000, 0.00000], [ 3, 0.20000, 0.00000]], [ 0.17453, [ 3, -0.20000, 0.00321], [ 3, 0.63333, -0.01015]], [ 0.0, [ 3, -0.63333, 0.00000], [ 3, 0.00000, 0.00000]]])
    
    names.append('LHipPitch')
    times.append([0.5, 1.0, 1.4, 1.8, 2.4, 2.8, 3.3, 3.8, 4.5, 5.2, 5.9, 7.0])
    angles.append([ [ 0.25008, [ 3, -0.33333, 0.00000], [ 3, 0.33333, 0.00000]], [ -0.17453, [ 3, -0.33333, 0.00000], [ 3, 0.26667, 0.00000]], [ 0.37434, [ 3, -0.26667, 0.00000], [ 3, 0.26667, 0.00000]], [ -0.17453, [ 3, -0.26667, 0.27599], [ 3, 0.23333, -0.24149]], [ -1.17808, [ 3, -0.23333, 0.27150], [ 3, 0.16667, -0.19393]], [ -1.57080, [ 3, -0.16667, 0.00000], [ 3, 0.26667, 0.00000]], [ -0.85746, [ 3, -0.26667, -0.23750], [ 3, 0.30000, 0.26719]], [ -0.05672, [ 3, -0.30000, 0.00000], [ 3, 0.30000, 0.00000]], [ -0.45556, [ 3, -0.30000, 0.15970], [ 3, 0.20000, -0.10647]], [ -0.85521, [ 3, -0.20000, 0.00000], [ 3, 0.20000, 0.00000]], [ -0.83599, [ 3, -0.20000, -0.01922], [ 3, 0.63333, 0.06087]], [ -0.4, [ 3, -0.63333, 0.00000], [ 3, 0.00000, 0.00000]]])

    names.append('LKneePitch')
    times.append([0.5, 1.0, 1.4, 1.8, 2.4, 2.8, 3.3, 3.8, 4.5, 5.2, 5.9, 7.0])
    angles.append([ [ 0.09200, [ 3, -0.33333, 0.00000], [ 3, 0.33333, 0.00000]], [ 1.67552, [ 3, -0.33333, -0.35708], [ 3, 0.26667, 0.28566]], [ 2.02024, [ 3, -0.26667, 0.00000], [ 3, 0.26667, 0.00000]], [ 1.67552, [ 3, -0.26667, 0.16755], [ 3, 0.23333, -0.14661]], [ 1.07777, [ 3, -0.23333, 0.00000], [ 3, 0.16667, 0.00000]], [ 1.67552, [ 3, -0.16667, -0.13243], [ 3, 0.26667, 0.21189]], [ 2.11075, [ 3, -0.26667, -0.00135], [ 3, 0.30000, 0.00152]], [ 2.11227, [ 3, -0.30000, -0.00026], [ 3, 0.30000, 0.00026]], [ 2.11253, [ 3, -0.30000, 0.00000], [ 3, 0.20000, 0.00000]], [ 2.11253, [ 3, -0.20000, 0.00000], [ 3, 0.20000, 0.00000]], [ 2.11253, [ 3, -0.20000, 0.00000], [ 3, 0.63333, 0.00000]], [ 0.95, [ 3, -0.63333, 0.00000], [ 3, 0.00000, 0.00000]]])
    
    names.append('LAnklePitch')
    times.append([0.5, 1.0, 1.4, 1.8, 2.4, 2.8, 3.3, 3.8, 4.5, 5.2, 5.9, 7.0])
    angles.append([ [ 0.82525, [ 3, -0.33333, 0.00000], [ 3, 0.33333, 0.00000]], [ 0.24435, [ 3, -0.33333, 0.23692], [ 3, 0.26667, -0.18953]], [ -0.45411, [ 3, -0.26667, 0.00000], [ 3, 0.26667, 0.00000]], [ 0.24435, [ 3, -0.26667, -0.20117], [ 3, 0.23333, 0.17602]], [ 0.67748, [ 3, -0.23333, 0.00000], [ 3, 0.16667, 0.00000]], [ 0.66323, [ 3, -0.16667, 0.01425], [ 3, 0.26667, -0.02280]], [ -0.45564, [ 3, -0.26667, 0.27681], [ 3, 0.30000, -0.31141]], [ -1.10145, [ 3, -0.30000, 0.08806], [ 3, 0.30000, -0.08806]], [ -1.18952, [ 3, -0.30000, 0.00000], [ 3, 0.20000, 0.00000]], [ -1.18952, [ 3, -0.20000, 0.00000], [ 3, 0.20000, 0.00000]], [ -1.18952, [ 3, -0.20000, 0.00000], [ 3, 0.63333, 0.00000]], [ -0.55, [ 3, -0.63333, 0.00000], [ 3, 0.00000, 0.00000]]])

    names.append('LAnkleRoll')
    times.append([0.5, 1.0, 1.4, 1.8, 2.4, 2.8, 3.3, 3.8, 4.5, 5.2, 5.9, 7.0])
    angles.append([ [ -0.03371, [ 3, -0.33333, 0.00000], [ 3, 0.33333, 0.00000]], [ 0.00000, [ 3, -0.33333, 0.00000], [ 3, 0.26667, 0.00000]], [ -0.03371, [ 3, -0.26667, 0.00000], [ 3, 0.26667, 0.00000]], [ 0.00000, [ 3, -0.26667, -0.03371], [ 3, 0.23333, 0.02949]], [ 0.17109, [ 3, -0.23333, 0.00000], [ 3, 0.16667, 0.00000]], [ -0.10472, [ 3, -0.16667, 0.07267], [ 3, 0.26667, -0.11627]], [ -0.39573, [ 3, -0.26667, 0.00000], [ 3, 0.30000, 0.00000]], [ -0.09967, [ 3, -0.30000, -0.07312], [ 3, 0.30000, 0.07312]], [ 0.04299, [ 3, -0.30000, -0.04355], [ 3, 0.20000, 0.02904]], [ 0.11810, [ 3, -0.20000, 0.00000], [ 3, 0.20000, 0.00000]], [ 0.08727, [ 3, -0.20000, 0.02037], [ 3, 0.63333, -0.06450]], [ 0.0, [ 3, -0.63333, 0.00000], [ 3, 0.00000, 0.00000]]])

    names.append('RHipRoll')
    times.append([0.5, 1.0, 1.4, 1.8, 2.4, 2.8, 3.3, 3.8, 4.5, 5.2, 5.9, 7.0])
    angles.append([ [ -0.14876, [ 3, -0.33333, 0.00000], [ 3, 0.33333, 0.00000]], [ 0.00000, [ 3, -0.33333, 0.00000], [ 3, 0.26667, 0.00000]], [ -0.02297, [ 3, -0.26667, 0.00000], [ 3, 0.26667, 0.00000]], [ 0.00000, [ 3, -0.26667, -0.01509], [ 3, 0.23333, 0.01320]], [ 0.06190, [ 3, -0.23333, 0.00000], [ 3, 0.16667, 0.00000]], [ -0.54105, [ 3, -0.16667, 0.01080], [ 3, 0.26667, -0.01728]], [ -0.55833, [ 3, -0.26667, 0.01089], [ 3, 0.30000, -0.01225]], [ -0.61049, [ 3, -0.30000, 0.01137], [ 3, 0.30000, -0.01137]], [ -0.62657, [ 3, -0.30000, 0.00000], [ 3, 0.20000, 0.00000]], [ -0.29671, [ 3, -0.20000, -0.10152], [ 3, 0.20000, 0.10152]], [ -0.01745, [ 3, -0.20000, 0.00000], [ 3, 0.63333, 0.00000]], [ 0.0, [ 3, -0.63333, 0.00000], [ 3, 0.00000, 0.00000]]])

    names.append('RHipPitch')
    times.append([0.5, 1.0, 1.4, 1.8, 2.4, 2.8, 3.3, 3.8, 4.5, 5.2, 5.9, 7.0])
    angles.append([ [ 0.18557, [ 3, -0.33333, 0.00000], [ 3, 0.33333, 0.00000]], [ -0.17453, [ 3, -0.33333, 0.00000], [ 3, 0.26667, 0.00000]], [ 0.28221, [ 3, -0.26667, 0.00000], [ 3, 0.26667, 0.00000]], [ -0.17453, [ 3, -0.26667, 0.25720], [ 3, 0.23333, -0.22505]], [ -1.16456, [ 3, -0.23333, 0.27150], [ 3, 0.16667, -0.19393]], [ -1.57080, [ 3, -0.16667, 0.00000], [ 3, 0.26667, 0.00000]], [ -1.52484, [ 3, -0.26667, 0.00000], [ 3, 0.30000, 0.00000]], [ -1.56012, [ 3, -0.30000, 0.00000], [ 3, 0.30000, 0.00000]], [ -1.02974, [ 3, -0.30000, -0.13086], [ 3, 0.20000, 0.08724]], [ -0.90583, [ 3, -0.20000, 0.00000], [ 3, 0.20000, 0.00000]], [ -0.90583, [ 3, -0.20000, 0.00000], [ 3, 0.63333, 0.00000]], [ -0.4, [ 3, -0.63333, 0.00000], [ 3, 0.00000, 0.00000]]])
    
    names.append('RKneePitch')
    times.append([0.5, 1.0, 1.4, 1.8, 2.4, 2.8, 3.3, 3.8, 4.5, 5.2, 5.9, 7.0])
    angles.append([ [ 0.19179, [ 3, -0.33333, 0.00000], [ 3, 0.33333, 0.00000]], [ 1.67552, [ 3, -0.33333, -0.33975], [ 3, 0.26667, 0.27180]], [ 2.02645, [ 3, -0.26667, 0.00000], [ 3, 0.26667, 0.00000]], [ 1.67552, [ 3, -0.26667, 0.16579], [ 3, 0.23333, -0.14507]], [ 1.09386, [ 3, -0.23333, 0.00000], [ 3, 0.16667, 0.00000]], [ 1.67552, [ 3, -0.16667, 0.00000], [ 3, 0.26667, 0.00000]], [ 1.22111, [ 3, -0.26667, 0.09029], [ 3, 0.30000, -0.10158]], [ 1.09992, [ 3, -0.30000, 0.01607], [ 3, 0.30000, -0.01607]], [ 1.08385, [ 3, -0.30000, 0.01607], [ 3, 0.20000, -0.01071]], [ 0.87616, [ 3, -0.20000, 0.00000], [ 3, 0.20000, 0.00000]], [ 1.76278, [ 3, -0.20000, 0.00000], [ 3, 0.63333, 0.00000]], [ 0.95, [ 3, -0.63333, 0.00000], [ 3, 0.00000, 0.00000]]])

    names.append('RAnklePitch')
    times.append([0.5, 1.0, 1.4, 1.8, 2.4, 2.8, 3.3, 3.8, 4.5, 5.2, 5.9, 7.0])
    angles.append([ [ 0.46638, [ 3, -0.33333, 0.00000], [ 3, 0.33333, 0.00000]], [ 0.24435, [ 3, -0.33333, 0.15567], [ 3, 0.26667, -0.12454]], [ -0.37425, [ 3, -0.26667, 0.00000], [ 3, 0.26667, 0.00000]], [ 0.24435, [ 3, -0.26667, -0.19407], [ 3, 0.23333, 0.16981]], [ 0.71737, [ 3, -0.23333, 0.00000], [ 3, 0.16667, 0.00000]], [ 0.66323, [ 3, -0.16667, 0.00000], [ 3, 0.26667, 0.00000]], [ 0.78545, [ 3, -0.26667, 0.00000], [ 3, 0.30000, 0.00000]], [ 0.78392, [ 3, -0.30000, 0.00153], [ 3, 0.30000, -0.00153]], [ 0.44157, [ 3, -0.30000, 0.05760], [ 3, 0.20000, -0.03840]], [ 0.40317, [ 3, -0.20000, 0.03840], [ 3, 0.20000, -0.03840]], [ -0.57945, [ 3, -0.20000, 0.00000], [ 3, 0.63333, 0.00000]], [ -0.55, [ 3, -0.63333, 0.00000], [ 3, 0.00000, 0.00000]]])

    names.append('RAnkleRoll')
    times.append([0.5, 1.0, 1.4, 1.8, 2.4, 2.8, 3.3, 3.8, 4.5, 5.2, 5.9, 7.0])
    angles.append([ [ 0.16725, [ 3, -0.33333, 0.00000], [ 3, 0.33333, 0.00000]], [ 0.00000, [ 3, -0.33333, 0.03835], [ 3, 0.26667, -0.03068]], [ -0.03984, [ 3, -0.26667, 0.00000], [ 3, 0.26667, 0.00000]], [ 0.00000, [ 3, -0.26667, 0.00000], [ 3, 0.23333, 0.00000]], [ -0.16084, [ 3, -0.23333, 0.00000], [ 3, 0.16667, 0.00000]], [ 0.10472, [ 3, -0.16667, 0.00000], [ 3, 0.26667, 0.00000]], [ 0.00925, [ 3, -0.26667, 0.03182], [ 3, 0.30000, -0.03580]], [ -0.09813, [ 3, -0.30000, 0.00000], [ 3, 0.30000, 0.00000]], [ 0.44331, [ 3, -0.30000, -0.15555], [ 3, 0.20000, 0.10370]], [ 0.67960, [ 3, -0.20000, 0.00000], [ 3, 0.20000, 0.00000]], [ 0.27751, [ 3, -0.20000, 0.04933], [ 3, 0.63333, -0.15622]], [ 0.0, [ 3, -0.63333, 0.00000], [ 3, 0.00000, 0.00000]]])
    
    motion.angleInterpolationBezier(names, times, angles)

# Stand up from belly
def bellyToStand():
    names = list()
    times = list()
    angles = list()
    
    names.append('HeadYaw')
    times.append([0.5, 1.0, 1.7, 2.3, 2.9, 3.7, 4.4, 5.2, 6.5])
    angles.append([ [ 0.17453, [ 3, -0.33333, 0.00000], [ 3, 0.33333, 0.00000]], [ -0.22689, [ 3, -0.33333, 0.00000], [ 3, 0.46667, 0.00000]], [ 0.28623, [ 3, -0.46667, -0.01333], [ 3, 0.36667, 0.01047]], [ 0.29671, [ 3, -0.36667, 0.00000], [ 3, 0.43333, 0.00000]], [ -0.49567, [ 3, -0.43333, 0.00000], [ 3, 0.23333, 0.00000]], [ -0.29671, [ 3, -0.23333, -0.07104], [ 3, 0.36667, 0.11164]], [ 0.05236, [ 3, -0.36667, 0.00000], [ 3, 0.43333, 0.00000]], [ -0.39095, [ 3, -0.43333, 0.00000], [ 3, 0.36667, 0.00000]], [ 0.0, [ 3, -0.36667, 0.00000], [ 3, 0.00000, 0.00000]]])

    names.append('HeadPitch')
    times.append([0.5, 1.0, 1.7, 2.3, 2.9, 3.7, 4.4, 5.2, 6.5])
    angles.append([ [ -0.57683, [ 3, -0.33333, 0.00000], [ 3, 0.33333, 0.00000]], [ -0.54768, [ 3, -0.33333, -0.02915], [ 3, 0.46667, 0.04081]], [ 0.10734, [ 3, -0.46667, -0.19834], [ 3, 0.36667, 0.15584]], [ 0.51487, [ 3, -0.36667, 0.00000], [ 3, 0.43333, 0.00000]], [ 0.38048, [ 3, -0.43333, 0.01726], [ 3, 0.23333, -0.00930]], [ 0.37119, [ 3, -0.23333, 0.00930], [ 3, 0.36667, -0.01461]], [ -0.10472, [ 3, -0.36667, 0.13827], [ 3, 0.43333, -0.16341]], [ -0.53387, [ 3, -0.43333, 0.00000], [ 3, 0.36667, 0.00000]], [ -0.5, [ 3, -0.36667, 0.00000], [ 3, 0.00000, 0.00000]]])

    names.append('LShoulderPitch')
    times.append([0.2, 0.5, 0.8, 1.7, 2.3, 2.9, 3.7, 4.4, 5.2, 6.5])
    angles.append([ [2, [ 3, -0.33333, 0.00000], [ 3, 0.33333, 0.00000]], [ -0.02757, [ 3, -0.33333, 0.00000], [ 3, 0.33333, 0.00000]], [ -1.51146, [ 3, -0.33333, 0.00000], [ 3, 0.46667, 0.00000]], [ -1.25025, [ 3, -0.46667, -0.26120], [ 3, 0.36667, 0.20523]], [ 0.07206, [ 3, -0.36667, -0.38566], [ 3, 0.43333, 0.45578]], [ 1.27409, [ 3, -0.43333, 0.00000], [ 3, 0.23333, 0.00000]], [ 0.75573, [ 3, -0.23333, 0.00333], [ 3, 0.36667, -0.00524]], [ 0.75049, [ 3, -0.36667, 0.00000], [ 3, 0.43333, 0.00000]], [ 1.29154, [ 3, -0.43333, -0.15226], [ 3, 0.36667, 0.12884]], [ 1.2, [ 3, -0.36667, 0.00000], [ 3, 0.00000, 0.00000]]])

    names.append('LShoulderRoll')
    times.append([0.3, 1.0, 1.7, 2.3, 2.9, 3.7, 4.4, 5.2, 6.5])
    angles.append([ [ 1.55390, [ 3, -0.33333, 0.00000], [ 3, 0.33333, 0.00000]], [ 0.01683, [ 3, -0.33333, 0.00000], [ 3, 0.46667, 0.00000]], [ 0.07666, [ 3, -0.46667, 0.00000], [ 3, 0.36667, 0.00000]], [ 0.07052, [ 3, -0.36667, 0.00000], [ 3, 0.43333, 0.00000]], [ 0.15643, [ 3, -0.43333, -0.08590], [ 3, 0.23333, 0.04626]], [ 0.93899, [ 3, -0.23333, 0.00000], [ 3, 0.36667, 0.00000]], [ 0.67719, [ 3, -0.36667, 0.00000], [ 3, 0.43333, 0.00000]], [ 0.84648, [ 3, -0.43333, 0.00000], [ 3, 0.36667, 0.00000]], [ 0.2, [ 3, -0.36667, 0.00000], [ 3, 0.00000, 0.00000]]])

    names.append('LElbowYaw')
    times.append([0.5, 1.0, 1.7, 2.3, 2.9, 3.7, 4.4, 5.2, 6.5])
    angles.append([ [ -2.07694, [ 3, -0.33333, 0.00000], [ 3, 0.33333, 0.00000]], [ -1.58006, [ 3, -0.33333, 0.00000], [ 3, 0.46667, 0.00000]], [ -1.60461, [ 3, -0.46667, 0.02454], [ 3, 0.36667, -0.01928]], [ -1.78715, [ 3, -0.36667, 0.00000], [ 3, 0.43333, 0.00000]], [ -1.32695, [ 3, -0.43333, -0.11683], [ 3, 0.23333, 0.06291]], [ -1.24791, [ 3, -0.23333, -0.04593], [ 3, 0.36667, 0.07218]], [ -0.97260, [ 3, -0.36667, -0.01072], [ 3, 0.43333, 0.01267]], [ -0.95993, [ 3, -0.43333, 0.00000], [ 3, 0.36667, 0.00000]], [ 0, [ 3, -0.36667, 0.00000], [ 3, 0.00000, 0.00000]]])

    names.append('LElbowRoll')
    times.append([0.5, 1.0, 1.7, 2.3, 2.9, 3.7, 4.4, 5.2, 6.5])
    angles.append([ [ -0.00873, [ 3, -0.33333, 0.00000], [ 3, 0.33333, 0.00000]], [ -0.35278, [ 3, -0.33333, 0.08741], [ 3, 0.46667, -0.12238]], [ -0.63810, [ 3, -0.46667, 0.09306], [ 3, 0.36667, -0.07312]], [ -0.85133, [ 3, -0.36667, 0.13944], [ 3, 0.43333, -0.16480]], [ -1.55083, [ 3, -0.43333, 0.00000], [ 3, 0.23333, 0.00000]], [ -0.73304, [ 3, -0.23333, 0.00000], [ 3, 0.36667, 0.00000]], [ -0.73653, [ 3, -0.36667, 0.00349], [ 3, 0.43333, -0.00413]], [ -1.15506, [ 3, -0.43333, 0.00000], [ 3, 0.36667, 0.00000]], [ 0, [ 3, -0.36667, 0.00000], [ 3, 0.00000, 0.00000]]])

    names.append('RShoulderPitch')
    times.append([0.2, 0.5, 1.0, 1.7, 2.3, 2.9, 3.7, 4.4, 5.2, 6.5])
    angles.append([ [2, [ 3, -0.33333, 0.00000], [ 3, 0.33333, 0.00000]], [ -0.02757, [ 3, -0.33333, 0.00000], [ 3, 0.33333, 0.00000]], [ -1.51146, [ 3, -0.33333, 0.00000], [ 3, 0.46667, 0.00000]], [ -1.22256, [ 3, -0.46667, -0.23805], [ 3, 0.36667, 0.18704]], [ -0.23619, [ 3, -0.36667, -0.22007], [ 3, 0.43333, 0.26008]], [ 0.21787, [ 3, -0.43333, -0.14857], [ 3, 0.23333, 0.08000]], [ 0.44950, [ 3, -0.23333, -0.09028], [ 3, 0.36667, 0.14187]], [ 0.91431, [ 3, -0.36667, -0.03894], [ 3, 0.43333, 0.04602]], [ 0.96033, [ 3, -0.43333, -0.04602], [ 3, 0.36667, 0.03894]], [ 1.2, [ 3, -0.36667, 0.00000], [ 3, 0.00000, 0.00000]]])

    names.append('RShoulderRoll')
    times.append([0.3, 1.0, 1.7, 2.3, 2.9, 3.7, 4.4, 5.2, 6.5])
    angles.append([ [ -1.53558, [ 3, -0.33333, 0.00000], [ 3, 0.33333, 0.00000]], [ -0.19199, [ 3, -0.33333, -0.07793], [ 3, 0.46667, 0.10911]], [ -0.08288, [ 3, -0.46667, 0.00000], [ 3, 0.36667, 0.00000]], [ -0.08288, [ 3, -0.36667, 0.00000], [ 3, 0.43333, 0.00000]], [ -0.22707, [ 3, -0.43333, 0.00000], [ 3, 0.23333, 0.00000]], [ -0.18259, [ 3, -0.23333, -0.02831], [ 3, 0.36667, 0.04448]], [ -0.00870, [ 3, -0.36667, 0.00000], [ 3, 0.43333, 0.00000]], [ -0.13197, [ 3, -0.43333, 0.01994], [ 3, 0.36667, -0.01687]], [ -0.2, [ 3, -0.36667, 0.00000], [ 3, 0.00000, 0.00000]]])

    names.append('RElbowYaw')
    times.append([0.5, 1.0, 1.7, 2.3, 2.9, 3.7, 4.4, 5.2, 6.5])
    angles.append([ [ 2.07694, [ 3, -0.33333, 0.00000], [ 3, 0.33333, 0.00000]], [ 1.56157, [ 3, -0.33333, 0.00000], [ 3, 0.46667, 0.00000]], [ 1.61373, [ 3, -0.46667, -0.02319], [ 3, 0.36667, 0.01822]], [ 1.68582, [ 3, -0.36667, -0.05296], [ 3, 0.43333, 0.06259]], [ 1.96041, [ 3, -0.43333, 0.00000], [ 3, 0.23333, 0.00000]], [ 1.95121, [ 3, -0.23333, 0.00920], [ 3, 0.36667, -0.01445]], [ 0.66571, [ 3, -0.36667, 0.22845], [ 3, 0.43333, -0.26998]], [ 0.39573, [ 3, -0.43333, 0.00000], [ 3, 0.36667, 0.00000]], [ 0, [ 3, -0.36667, 0.00000], [ 3, 0.00000, 0.00000]]])

    names.append('RElbowRoll')
    times.append([0.5, 1.0, 1.7, 2.3, 2.9, 3.7, 4.4, 5.2, 6.5])
    angles.append([ [ 0.10472, [ 3, -0.33333, 0.00000], [ 3, 0.33333, 0.00000]], [ 0.38201, [ 3, -0.33333, -0.07367], [ 3, 0.46667, 0.10313]], [ 0.63512, [ 3, -0.46667, -0.21934], [ 3, 0.36667, 0.17234]], [ 1.55705, [ 3, -0.36667, 0.00000], [ 3, 0.43333, 0.00000]], [ 0.00870, [ 3, -0.43333, 0.00000], [ 3, 0.23333, 0.00000]], [ 0.00870, [ 3, -0.23333, 0.00000], [ 3, 0.36667, 0.00000]], [ 0.42343, [ 3, -0.36667, -0.09786], [ 3, 0.43333, 0.11566]], [ 0.64926, [ 3, -0.43333, 0.00000], [ 3, 0.36667, 0.00000]], [ 0.0, [ 3, -0.36667, 0.00000], [ 3, 0.00000, 0.00000]]])

    names.append('LHipYawPitch')
    times.append([0.5, 1.0, 1.7, 2.3, 2.9, 3.7, 4.4, 5.2, 6.5])
    angles.append([ [ -0.03371, [ 3, -0.33333, 0.00000], [ 3, 0.33333, 0.00000]], [ 0.03491, [ 3, -0.33333, 0.00000], [ 3, 0.46667, 0.00000]], [ -0.43561, [ 3, -0.46667, 0.15197], [ 3, 0.36667, -0.11941]], [ -0.77923, [ 3, -0.36667, 0.09257], [ 3, 0.43333, -0.10940]], [ -1.04154, [ 3, -0.43333, 0.07932], [ 3, 0.23333, -0.04271]], [ -1.530, [ 3, -0.23333, 0.00000], [ 3, 0.36667, 0.00000]], [ -1, [ 3, -0.36667, 0.00000], [ 3, 0.43333, 0.00000]], [ -0.56754, [ 3, -0.43333, -0.16414], [ 3, 0.36667, 0.13889]], [ 0.0, [ 3, -0.36667, 0.00000], [ 3, 0.00000, 0.00000]]])

    names.append('LHipRoll')
    times.append([0.5, 1.0, 1.7, 2.3, 2.9, 3.7, 4.4, 5.2, 6.5])
    angles.append([ [ 0.06294, [ 3, -0.33333, 0.00000], [ 3, 0.33333, 0.00000]], [ 0.00004, [ 3, -0.33333, 0.00000], [ 3, 0.46667, 0.00000]], [ 0.00158, [ 3, -0.46667, 0.00000], [ 3, 0.36667, 0.00000]], [ -0.37732, [ 3, -0.36667, 0.00000], [ 3, 0.43333, 0.00000]], [ -0.29755, [ 3, -0.43333, 0.00000], [ 3, 0.23333, 0.00000]], [ -0.29755, [ 3, -0.23333, 0.00000], [ 3, 0.36667, 0.00000]], [ 0.19486, [ 3, -0.36667, 0.00000], [ 3, 0.43333, 0.00000]], [ 0.12736, [ 3, -0.43333, 0.00000], [ 3, 0.36667, 0.00000]], [ 0.0, [ 3, -0.36667, 0.00000], [ 3, 0.00000, 0.00000]]])

    names.append('LHipPitch')
    times.append([0.5, 1.0, 1.7, 2.3, 2.9, 3.7, 4.4, 5.2, 6.5])
    angles.append([ [ 0.06140, [ 3, -0.33333, 0.00000], [ 3, 0.33333, 0.00000]], [ 0.00004, [ 3, -0.33333, 0.06136], [ 3, 0.46667, -0.08590]], [ -1.56924, [ 3, -0.46667, 0.00000], [ 3, 0.36667, 0.00000]], [ -1.28085, [ 3, -0.36667, -0.08132], [ 3, 0.43333, 0.09611]], [ -1.03694, [ 3, -0.43333, 0.00000], [ 3, 0.23333, 0.00000]], [ -1.15966, [ 3, -0.23333, 0.01464], [ 3, 0.36667, -0.02301]], [ -1.18267, [ 3, -0.36667, 0.01687], [ 3, 0.43333, -0.01994]], [ -1.27011, [ 3, -0.43333, 0.00000], [ 3, 0.36667, 0.00000]], [ -0.4, [ 3, -0.36667, 0.00000], [ 3, 0.00000, 0.00000]]])

    names.append('LKneePitch')
    times.append([0.5, 1.0, 1.7, 2.3, 2.9, 3.7, 4.4, 5.2, 6.5])
    angles.append([ [ 0.12043, [ 3, -0.33333, 0.00000], [ 3, 0.33333, 0.00000]], [ 1.98968, [ 3, -0.33333, -0.08775], [ 3, 0.46667, 0.12285]], [ 2.11253, [ 3, -0.46667, 0.00000], [ 3, 0.36667, 0.00000]], [ 0.28221, [ 3, -0.36667, 0.00000], [ 3, 0.43333, 0.00000]], [ 0.40493, [ 3, -0.43333, 0.00000], [ 3, 0.23333, 0.00000]], [ 0.35738, [ 3, -0.23333, 0.00000], [ 3, 0.36667, 0.00000]], [ 0.71940, [ 3, -0.36667, -0.25311], [ 3, 0.43333, 0.29913]], [ 2.01409, [ 3, -0.43333, 0.00000], [ 3, 0.36667, 0.00000]], [ 0.95, [ 3, -0.36667, 0.00000], [ 3, 0.00000, 0.00000]]])

    names.append('LAnklePitch')
    times.append([0.5, 1.0, 1.7, 2.3, 2.9, 3.7, 4.4, 5.2, 6.5])
    angles.append([ [ 0.92189, [ 3, -0.33333, 0.00000], [ 3, 0.33333, 0.00000]], [ -1.02974, [ 3, -0.33333, 0.08628], [ 3, 0.46667, -0.12080]], [ -1.15054, [ 3, -0.46667, 0.00000], [ 3, 0.36667, 0.00000]], [ 0.21625, [ 3, -0.36667, -0.28428], [ 3, 0.43333, 0.33597]], [ 0.71020, [ 3, -0.43333, -0.15307], [ 3, 0.23333, 0.08242]], [ 0.92275, [ 3, -0.23333, 0.00000], [ 3, 0.36667, 0.00000]], [ 0.82525, [ 3, -0.36667, 0.09750], [ 3, 0.43333, -0.11522]], [ -0.50166, [ 3, -0.43333, 0.00000], [ 3, 0.36667, 0.00000]], [ -0.55, [ 3, -0.36667, 0.00000], [ 3, 0.00000, 0.00000]]])

    names.append('LAnkleRoll')
    times.append([0.5, 1.0, 1.7, 2.3, 2.9, 3.7, 4.4, 5.2, 6.5])
    angles.append([ [ -0.00149, [ 3, -0.33333, 0.00000], [ 3, 0.33333, 0.00000]], [ 0.00004, [ 3, -0.33333, 0.00000], [ 3, 0.46667, 0.00000]], [ -0.00149, [ 3, -0.46667, 0.00153], [ 3, 0.36667, -0.00121]], [ -0.45249, [ 3, -0.36667, 0.00000], [ 3, 0.43333, 0.00000]], [ -0.30062, [ 3, -0.43333, -0.07246], [ 3, 0.23333, 0.03901]], [ -0.11808, [ 3, -0.23333, -0.03361], [ 3, 0.36667, 0.05281]], [ -0.04138, [ 3, -0.36667, 0.00000], [ 3, 0.43333, 0.00000]], [ -0.12114, [ 3, -0.43333, 0.01632], [ 3, 0.36667, -0.01381]], [ 0.0, [ 3, -0.36667, 0.00000], [ 3, 0.00000, 0.00000]]])

    names.append('RHipRoll')
    times.append([0.5, 1.0, 1.7, 2.3, 2.9, 3.7, 4.4, 5.2, 6.5])
    angles.append([ [ 0.03142, [ 3, -0.33333, 0.00000], [ 3, 0.33333, 0.00000]], [ 0.00004, [ 3, -0.33333, 0.00000], [ 3, 0.46667, 0.00000]], [ 0.00158, [ 3, -0.46667, -0.00153], [ 3, 0.36667, 0.00121]], [ 0.31144, [ 3, -0.36667, 0.00000], [ 3, 0.43333, 0.00000]], [ 0.25469, [ 3, -0.43333, 0.00000], [ 3, 0.23333, 0.00000]], [ 0.32065, [ 3, -0.23333, 0.00000], [ 3, 0.36667, 0.00000]], [ 0.22707, [ 3, -0.36667, 0.06047], [ 3, 0.43333, -0.07146]], [ -0.07512, [ 3, -0.43333, 0.00000], [ 3, 0.36667, 0.00000]], [ 0, [ 3, -0.36667, 0.00000], [ 3, 0.00000, 0.00000]]])

    names.append('RHipPitch')
    times.append([0.5, 1.0, 1.7, 2.3, 2.9, 3.7, 4.4, 5.2, 6.5])
    angles.append([ [ 0.07666, [ 3, -0.33333, 0.00000], [ 3, 0.33333, 0.00000]], [ -0.00004, [ 3, -0.33333, 0.07670], [ 3, 0.46667, -0.10738]], [ -1.57699, [ 3, -0.46667, 0.10738], [ 3, 0.36667, -0.08437]], [ -1.66136, [ 3, -0.36667, 0.00000], [ 3, 0.43333, 0.00000]], [ -1.19963, [ 3, -0.43333, 0.00000], [ 3, 0.23333, 0.00000]], [ -1.59847, [ 3, -0.23333, 0.00000], [ 3, 0.36667, 0.00000]], [ -0.32218, [ 3, -0.36667, 0.00000], [ 3, 0.43333, 0.00000]], [ -0.71028, [ 3, -0.43333, 0.00000], [ 3, 0.36667, 0.00000]], [ -0.4, [ 3, -0.36667, 0.00000], [ 3, 0.00000, 0.00000]]])

    names.append('RKneePitch')
    times.append([0.5, 1.0, 1.7, 2.3, 2.9, 3.7, 4.4, 5.2, 6.5])
    angles.append([ [ -0.07819, [ 3, -0.33333, 0.00000], [ 3, 0.33333, 0.00000]], [ 1.98968, [ 3, -0.33333, -0.06900], [ 3, 0.46667, 0.09660]], [ 2.08628, [ 3, -0.46667, 0.00000], [ 3, 0.36667, 0.00000]], [ 1.74267, [ 3, -0.36667, 0.00000], [ 3, 0.43333, 0.00000]], [ 2.12019, [ 3, -0.43333, 0.00000], [ 3, 0.23333, 0.00000]], [ 2.12019, [ 3, -0.23333, 0.00000], [ 3, 0.36667, 0.00000]], [ 2.12019, [ 3, -0.36667, 0.00000], [ 3, 0.43333, 0.00000]], [ 2.12019, [ 3, -0.43333, 0.00000], [ 3, 0.36667, 0.00000]], [ 0.95, [ 3, -0.36667, 0.00000], [ 3, 0.00000, 0.00000]]])

    names.append('RAnklePitch')
    times.append([0.5, 1.0, 1.7, 2.3, 2.9, 3.7, 4.4, 5.2, 6.5])
    angles.append([ [ 0.92965, [ 3, -0.33333, 0.00000], [ 3, 0.33333, 0.00000]], [ -1.02974, [ 3, -0.33333, 0.07746], [ 3, 0.46667, -0.10844]], [ -1.13819, [ 3, -0.46667, 0.02925], [ 3, 0.36667, -0.02298]], [ -1.18645, [ 3, -0.36667, 0.00000], [ 3, 0.43333, 0.00000]], [ -1.18645, [ 3, -0.43333, 0.00000], [ 3, 0.23333, 0.00000]], [ -0.58901, [ 3, -0.23333, 0.00000], [ 3, 0.36667, 0.00000]], [ -1.18645, [ 3, -0.36667, 0.00000], [ 3, 0.43333, 0.00000]], [ -1.18645, [ 3, -0.43333, 0.00000], [ 3, 0.36667, 0.00000]], [ -0.55, [ 3, -0.36667, 0.00000], [ 3, 0.00000, 0.00000]]])

    names.append('RAnkleRoll')
    times.append([0.5, 1.0, 1.7, 2.3, 2.9, 3.7, 4.4, 5.2, 6.5])
    angles.append([ [ 0.18850, [ 3, -0.33333, 0.00000], [ 3, 0.33333, 0.00000]], [ 0.00004, [ 3, -0.33333, 0.00000], [ 3, 0.46667, 0.00000]], [ 0.00618, [ 3, -0.46667, 0.00000], [ 3, 0.36667, 0.00000]], [ -0.00456, [ 3, -0.36667, 0.01074], [ 3, 0.43333, -0.01269]], [ -0.09813, [ 3, -0.43333, 0.00000], [ 3, 0.23333, 0.00000]], [ -0.01376, [ 3, -0.23333, 0.00000], [ 3, 0.36667, 0.00000]], [ -0.09507, [ 3, -0.36667, 0.00000], [ 3, 0.43333, 0.00000]], [ 0.03532, [ 3, -0.43333, -0.02825], [ 3, 0.36667, 0.02390]], [ 0.0, [ 3, -0.36667, 0.00000], [ 3, 0.00000, 0.00000]]])

    motion.angleInterpolationBezier(names, times, angles)
    
# keeper stance
def stance():
    names = list()
    angles = list()
    times = list()
    
    names.append('LLeg')
    angles.extend([[0],  [0],  [-0.900], [2.12], [-1.2],  [0]  ])
    times.extend([[0.7], [0.7], [0.7],    [0.7],     [0.7],     [0.7]])
    
    names.append('RLeg')
    angles.extend([[0], [0], [-0.900], [2.12], [-1.2], [0]  ])
    times.extend([[0.7], [0.7],  [0.7],    [0.7],    [0.7],     [0.7]])
    
    names.append('RArm')
    angles.extend([[1.2], [-0.1], [0],   [0] ])
    times.extend([ [0.7],  [0.7], [0.7], [0.7]])

    names.append('LArm')
    angles.extend([[1.2],   [0.1],   [0],     [0]  ])
    times.extend([[0.7], [0.7], [0.7], [0.7]])
    
    motion.post.angleInterpolation(names, angles, times, True)
    
def killKnees():
    if motion.getStiffnesses ('RKneePitch') > 0.2:
        motion.setStiffnesses(['RKneePitch','LKneePitch','RAnklePitch','LAnklePitch'], 0)
    
def stiffKnees():
    motion.setStiffnesses(['RKneePitch','LKneePitch','RAnklePitch','LAnklePitch'], 0.95)
    while motion.getStiffnesses ('RKneePitch') < 0.95:
        pass

def killWalk():
    walkTo((0.00001, 0, 0))
    
# RETURNS
def getPose():
    return pose.getActualPoseAndTime()[0]
    
def isWalking():
    return motion.walkIsActive()

def getNormalPose():
    anglelist = motion.getAngles(['RHipPitch','RKneePitch', 'RAnklePitch'],False)
    return pose.getActualPoseAndTime()[0] == 'Stand' and -0.41 <= anglelist[0] <= -0.39 and 0.94 <= anglelist[1] <= 0.96 and -0.56 <= anglelist[2] <= -0.54
    
def getKeepNormalPose():
    anglelist = motion.getAngles(['RHipPitch','RKneePitch', 'RAnklePitch'],False)
    return pose.getActualPoseAndTime()[0] == 'Stand' and -0.01 <= anglelist[0] <= 0.01 and -0.01 <= anglelist[1] <= 0.01 and -0.01 <= anglelist[2] <= 0.01
    
def getHeadPos():
    return motion.getAngles(['HeadYaw', 'HeadPitch'], True)
    
def getStance():    
    [ankle] = motion.getAngles('RAnklePitch', False)
    return -1.2 <= ankle <= -1.1
        
def killAllTasks():
    normalPose()
    setHead((0,0))
    
def kill():
    motion.setStiffnesses('Body', 0)